From 9d3c19fee87b379e695a9351e0a4bea45b930255 Mon Sep 17 00:00:00 2001 From: Saraansh Wadkar Date: Sat, 17 Feb 2024 19:08:41 -0500 Subject: [PATCH 1/6] add vision sim --- simgui-ds.json | 25 ++++--- .../lib/logging/DoubleArrayConversions.kt | 7 +- .../com/team4099/robot2023/RobotContainer.kt | 9 +-- .../config/constants/FieldConstants.kt | 16 +++- .../robot2023/subsystems/vision/Vision.kt | 7 +- .../subsystems/vision/camera/CameraIO.kt | 17 ++--- .../vision/camera/CameraIOPhotonvision.kt | 75 +++++++++---------- .../subsystems/vision/camera/CameraIOSim.kt | 68 +++++++++++++++++ 8 files changed, 150 insertions(+), 74 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOSim.kt diff --git a/simgui-ds.json b/simgui-ds.json index 97f4895e..de69ad8b 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "Keyboard 0 Settings": { + "window": { + "visible": true + } + }, "System Joysticks": { "window": { "visible": false @@ -20,9 +25,17 @@ "decayRate": 0.0, "incKey": 82, "keyRate": 0.009999999776482582 + }, + { + "decKey": 75, + "incKey": 74 + }, + { + "decKey": 76, + "incKey": 73 } ], - "axisCount": 3, + "axisCount": 5, "buttonCount": 4, "buttonKeys": [ 90, @@ -45,16 +58,6 @@ "povCount": 1 }, { - "axisConfig": [ - { - "decKey": 74, - "incKey": 76 - }, - { - "decKey": 73, - "incKey": 75 - } - ], "axisCount": 2, "buttonCount": 4, "buttonKeys": [ diff --git a/src/main/kotlin/com/team4099/lib/logging/DoubleArrayConversions.kt b/src/main/kotlin/com/team4099/lib/logging/DoubleArrayConversions.kt index 07f4b7cd..9a52c34f 100644 --- a/src/main/kotlin/com/team4099/lib/logging/DoubleArrayConversions.kt +++ b/src/main/kotlin/com/team4099/lib/logging/DoubleArrayConversions.kt @@ -24,9 +24,10 @@ fun Pose3d.toDoubleArray(): Array { this.x.inMeters, this.y.inMeters, this.z.inMeters, - this.rotation.x.inRadians, - this.rotation.y.inRadians, - this.rotation.z.inRadians + this.rotation.quaternion.w.inRadians, + this.rotation.quaternion.x.inMeters, + this.rotation.quaternion.y.inMeters, + this.rotation.quaternion.z.inMeters ) } diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 4c23d008..ed692b60 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -27,6 +27,8 @@ import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO import com.team4099.robot2023.subsystems.superstructure.Request import com.team4099.robot2023.subsystems.superstructure.Superstructure import com.team4099.robot2023.subsystems.vision.Vision +import com.team4099.robot2023.subsystems.vision.camera.CameraIOPhotonvision +import com.team4099.robot2023.subsystems.vision.camera.CameraIOSim import com.team4099.robot2023.subsystems.wrist.Wrist import com.team4099.robot2023.subsystems.wrist.WristIO import com.team4099.robot2023.subsystems.wrist.WristIOSim @@ -74,12 +76,7 @@ object RobotContainer { } else { // Simulation implementations drivetrain = Drivetrain(object : GyroIO {}, DrivetrainIOSim) - vision = - Vision( - CameraIOPhotonvision("parakeet_1"), - CameraIOPhotonvision("parakeet_2"), - CameraIOPhotonvision("parakeet_3") - ) + vision = Vision(CameraIOSim("skrt")) limelight = LimelightVision(object : LimelightVisionIO {}) intake = Intake(IntakeIOSim) feeder = Feeder(FeederIOSim) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt index 247d4573..2507e757 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FieldConstants.kt @@ -2,14 +2,28 @@ package com.team4099.robot2023.config.constants import org.team4099.lib.apriltag.AprilTag import org.team4099.lib.geometry.Pose3d +import org.team4099.lib.geometry.Rotation3d +import org.team4099.lib.geometry.Translation3d import org.team4099.lib.units.base.feet import org.team4099.lib.units.base.inMeters +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.radians object FieldConstants { val fieldLength = 54.feet val fieldWidth = 26.feet - val aprilTags: List = listOf() + val aprilTags: List = + listOf( + AprilTag( + 0, + Pose3d( + Translation3d(1.0.meters, 5.0.meters, 2.0.meters), + Rotation3d(0.0.radians, 0.0.radians, 180.degrees) + ) + ) + ) val homeAprilTags: List = listOf() val wpilibAprilTags = diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt index 88267673..147e95b6 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt @@ -14,15 +14,12 @@ import org.team4099.lib.geometry.Pose2d import org.team4099.lib.geometry.Pose2dWPILIB import org.team4099.lib.geometry.Pose3d import org.team4099.lib.geometry.Pose3dWPILIB -import org.team4099.lib.geometry.Quaternion -import org.team4099.lib.geometry.Rotation3d import org.team4099.lib.units.base.Time import org.team4099.lib.units.base.inMeters import org.team4099.lib.units.base.inMilliseconds import org.team4099.lib.units.base.meters import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.degrees -import org.team4099.lib.units.derived.radians import java.util.function.Consumer import java.util.function.Supplier import kotlin.math.pow @@ -86,6 +83,7 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { val visionUpdates = mutableListOf() for (instance in io.indices) { + val currentPose = fieldFramePoseSupplier.get() lastFrameTimes[instance] = Clock.fpgaTime val timestamp = inputs[instance].timestamp @@ -120,7 +118,7 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { val thetaStdDev = thetaStdDev.get() * averageDistance.inMeters.pow(2) / tagPoses.size visionUpdates.add( - PoseEstimator.TimestampedVisionUpdate( + TimestampedVisionUpdate( timestamp, robotPose, VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev) ) ) @@ -140,7 +138,6 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { *tagPoses.map { it.pose3d }.toTypedArray() ) - if (inputs[instance].timestamp == 0.0.seconds) { // prolly wrong lol Logger.recordOutput( "Vision/${VisionConstants.CAMERA_NAMES[instance]}/estimatedRobotPose", diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIO.kt index 83aa1187..fb5f4264 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIO.kt @@ -1,5 +1,6 @@ package com.team4099.robot2023.subsystems.vision.camera +import com.team4099.lib.logging.toDoubleArray import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.inputs.LoggableInputs import org.team4099.lib.geometry.Pose3d @@ -16,20 +17,16 @@ interface CameraIO { override fun toLog(table: LogTable?) { table?.put("timestampSeconds", timestamp.inSeconds) - table?.put("frame", frame.pose3d) + table?.put("frame", frame.toDoubleArray().toDoubleArray()) table?.put("fps", fps) - table?.put("usedTargets", usedTargets) + table?.put("usedTargets", usedTargets.toIntArray()) } override fun fromLog(table: LogTable?) { - table?.get("timestampSeconds", 0.0)?.let { - timestamp = it.seconds - } - table?.get("frame", Pose3dWPILIB())?.let { - frame = Pose3d(it) - } - table?.get("fps", 0.0) - table?.get("usedTargets", listOf()) + table?.get("timestampSeconds", 0.0)?.let { timestamp = it.seconds } + table?.get("frame", Pose3dWPILIB())?.let { frame = Pose3d(it[0]) } + table?.get("fps", 0.0).let { fps = it ?: 0.0 } + table?.get("usedTargets", IntArray(0)).let { usedTargets = it?.toList() ?: listOf() } } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOPhotonvision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOPhotonvision.kt index 9026d901..e327a2b5 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOPhotonvision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOPhotonvision.kt @@ -5,51 +5,50 @@ import org.photonvision.EstimatedRobotPose import org.photonvision.PhotonCamera import org.photonvision.PhotonPoseEstimator import org.photonvision.PhotonPoseEstimator.PoseStrategy -import org.team4099.lib.around import org.team4099.lib.geometry.Pose3d -import org.team4099.lib.geometry.Pose3dWPILIB import org.team4099.lib.geometry.Transform3dWPILIB import org.team4099.lib.units.base.Time import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.base.seconds import org.team4099.lib.units.micro -import org.team4099.lib.units.milli -import java.util.* - - -class CameraIOPhotonvision(private val identifier: String): CameraIO { - - private val photonEstimator: PhotonPoseEstimator - private val camera: PhotonCamera - private var lastEstTimestamp: Time = 0.0.seconds - - init { - camera = PhotonCamera(identifier) - photonEstimator = PhotonPoseEstimator( - FieldConstants.wpilibFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, camera, Transform3dWPILIB() - ) - photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY) - +import java.util.Optional + +class CameraIOPhotonvision(private val identifier: String) : CameraIO { + + private val photonEstimator: PhotonPoseEstimator + private val camera: PhotonCamera + private var lastEstTimestamp: Time = 0.0.seconds + + init { + camera = PhotonCamera(identifier) + photonEstimator = + PhotonPoseEstimator( + FieldConstants.wpilibFieldLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + camera, + Transform3dWPILIB() + ) + photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY) + } + + override fun updateInputs(inputs: CameraIO.CameraInputs) { + val visionEst: Optional? = photonEstimator.update() + val poseEst = visionEst?.get()?.let { Pose3d(it.estimatedPose) } + val latestTimestamp = visionEst?.get()?.timestampSeconds?.seconds + if (latestTimestamp != null) { + if ((latestTimestamp - lastEstTimestamp).absoluteValue > 10.micro.seconds) { + inputs.fps = 1 / (latestTimestamp - lastEstTimestamp).inSeconds + lastEstTimestamp = latestTimestamp + } } - override fun updateInputs(inputs: CameraIO.CameraInputs) { - val visionEst: Optional? = photonEstimator.update() - val poseEst = visionEst?.get()?.let { Pose3d(it.estimatedPose) } - val latestTimestamp = visionEst?.get()?.timestampSeconds?.seconds - if (latestTimestamp != null) { - if ((latestTimestamp - lastEstTimestamp).absoluteValue > 10.micro.seconds) { - inputs.fps = 1/(latestTimestamp - lastEstTimestamp).inSeconds - lastEstTimestamp = latestTimestamp - } - } - - if (visionEst != null) { - inputs.usedTargets = visionEst.get().targetsUsed.map { it.fiducialId } - } + if (visionEst != null) { + inputs.usedTargets = visionEst.get().targetsUsed.map { it.fiducialId } + } - inputs.timestamp = lastEstTimestamp - if (poseEst != null) { - inputs.frame = poseEst - } + inputs.timestamp = lastEstTimestamp + if (poseEst != null) { + inputs.frame = poseEst } -} \ No newline at end of file + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOSim.kt new file mode 100644 index 00000000..91dc86ed --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOSim.kt @@ -0,0 +1,68 @@ +package com.team4099.robot2023.subsystems.vision.camera + +import com.team4099.lib.hal.Clock +import com.team4099.lib.logging.toDoubleArray +import com.team4099.robot2023.config.constants.FieldConstants +import com.team4099.robot2023.config.constants.VisionConstants +import com.team4099.robot2023.util.toPose3d +import com.team4099.robot2023.util.toTransform3d +import edu.wpi.first.networktables.DoubleArraySubscriber +import edu.wpi.first.networktables.NetworkTableInstance +import org.littletonrobotics.junction.Logger +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.geometry.Pose3d +import org.team4099.lib.geometry.Rotation3d +import org.team4099.lib.geometry.Translation2d +import org.team4099.lib.geometry.Translation3d +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.base.seconds +import org.team4099.lib.units.derived.radians + +class CameraIOSim(val id: String) : CameraIO { + private val poseSubscriber: DoubleArraySubscriber = + NetworkTableInstance.getDefault() + .getTable("AdvantageKit") + .getSubTable("RealOutputs") + .getDoubleArrayTopic(VisionConstants.POSE_TOPIC_NAME) + .subscribe(DoubleArray(3)) + private val errorScaleConstant = 0.05 + + override fun updateInputs(inputs: CameraIO.CameraInputs) { + val drivePose = poseSubscriber.get(DoubleArray(3)).toPose2d().toPose3d() + + inputs.timestamp = Clock.fpgaTime - (Math.random() * 0.2).seconds + val robotTtag = drivePose.relativeTo(FieldConstants.aprilTags[0].pose) + inputs.frame = robotTtag.gaussianShenanigans() + val estimatedRobotPose = + FieldConstants.aprilTags[0].pose.transformBy(inputs.frame.toTransform3d()) + + Logger.recordOutput( + "Vision/$id/simEstimatedRobotPose", + estimatedRobotPose.toPose2d().toDoubleArray().toDoubleArray() + ) + Logger.recordOutput( + "Vision/$id/simTagPose", FieldConstants.aprilTags[0].pose.toDoubleArray().toDoubleArray() + ) + inputs.fps = Math.random() * 10 + 30 + inputs.usedTargets = listOf(0) + } + + private fun DoubleArray.toPose2d(): Pose2d { + return Pose2d(Translation2d(this[0].meters, this[1].meters), this[2].radians) + } + + private fun Pose3d.gaussianShenanigans(): Pose3d { + return Pose3d( + Translation3d( + this.translation.x * (1 - Math.random() * errorScaleConstant), + this.translation.y * (1 - Math.random() * errorScaleConstant), + this.translation.z * (1 - Math.random() * errorScaleConstant) + ), + Rotation3d( + this.rotation.x * (1 - Math.random() * errorScaleConstant), + this.rotation.y * (1 - Math.random() * errorScaleConstant), + this.rotation.z * (1 - Math.random() * errorScaleConstant) + ) + ) + } +} From d02e453cf525cb5bb8e35be8d626e77c52c50253 Mon Sep 17 00:00:00 2001 From: Parth Oza Date: Sat, 17 Feb 2024 17:15:53 -0800 Subject: [PATCH 2/6] Sim fixes --- .../kotlin/com/team4099/lib/math/GeomUtil.kt | 10 ++++++ .../com/team4099/robot2023/RobotContainer.kt | 10 +++--- .../config/constants/VisionConstants.kt | 5 +++ .../subsystems/drivetrain/drive/Drivetrain.kt | 4 +-- .../robot2023/subsystems/vision/Vision.kt | 35 +++++++++++-------- .../subsystems/vision/camera/CameraIO.kt | 4 +++ .../vision/camera/CameraIOPhotonvision.kt | 5 +-- .../subsystems/vision/camera/CameraIOSim.kt | 5 +-- 8 files changed, 53 insertions(+), 25 deletions(-) diff --git a/src/main/kotlin/com/team4099/lib/math/GeomUtil.kt b/src/main/kotlin/com/team4099/lib/math/GeomUtil.kt index c3de20af..5c0f3898 100644 --- a/src/main/kotlin/com/team4099/lib/math/GeomUtil.kt +++ b/src/main/kotlin/com/team4099/lib/math/GeomUtil.kt @@ -4,8 +4,10 @@ import org.team4099.lib.geometry.Pose2d import org.team4099.lib.geometry.Transform2d import org.team4099.lib.geometry.Translation2d import org.team4099.lib.geometry.Twist2d +import org.team4099.lib.units.base.inMeters import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRadians import org.team4099.lib.units.derived.radians /** @@ -47,3 +49,11 @@ fun Pose2d.asTransform2d(): Transform2d { fun Transform2d.asPose2d(): Pose2d { return Pose2d(this.translation, this.rotation) } + +fun Transform2d.asDoubleArray(): DoubleArray { + return doubleArrayOf(this.translation.x.inMeters, this.translation.y.inMeters, this.rotation.inRadians) +} + +fun Pose2d.asDoubleArray(): DoubleArray { + return doubleArrayOf(this.x.inMeters, this.y.inMeters, this.rotation.inRadians) +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index ed692b60..dfe00707 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -5,6 +5,7 @@ import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand import com.team4099.robot2023.config.ControlBoard import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.VisionConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOReal import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim @@ -32,7 +33,6 @@ import com.team4099.robot2023.subsystems.vision.camera.CameraIOSim import com.team4099.robot2023.subsystems.wrist.Wrist import com.team4099.robot2023.subsystems.wrist.WristIO import com.team4099.robot2023.subsystems.wrist.WristIOSim -import com.team4099.robot2023.subsystems.vision.camera.CameraIOPhotonvision import com.team4099.robot2023.util.driver.Ryan import edu.wpi.first.wpilibj.RobotBase import org.team4099.lib.smoothDeadband @@ -61,9 +61,9 @@ object RobotContainer { Vision( // object: CameraIO {} // CameraIONorthstar("northstar"), - CameraIOPhotonvision("parakeet_1"), - CameraIOPhotonvision("parakeet_2"), - CameraIOPhotonvision("parakeet_3"), + CameraIOPhotonvision("parakeet_1", VisionConstants.CAMERA_TRANSFORMS[0]), + CameraIOPhotonvision("parakeet_2", VisionConstants.CAMERA_TRANSFORMS[1]), + CameraIOPhotonvision("parakeet_3", VisionConstants.CAMERA_TRANSFORMS[2]), // CameraIONorthstar("right"), // CameraIONorthstar("backward") ) @@ -76,7 +76,7 @@ object RobotContainer { } else { // Simulation implementations drivetrain = Drivetrain(object : GyroIO {}, DrivetrainIOSim) - vision = Vision(CameraIOSim("skrt")) + vision = Vision(CameraIOSim("skrt", VisionConstants.SIM_CAMERA_TRANSFORM)) limelight = LimelightVision(object : LimelightVisionIO {}) intake = Intake(IntakeIOSim) feeder = Feeder(FeederIOSim) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt index bfce7c8c..ac5eca98 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt @@ -56,6 +56,11 @@ object VisionConstants { ) // camera facing leftward ) + val SIM_CAMERA_TRANSFORM = Transform3d( + Translation3d(0.inches, 0.inches, 0.inches), + Rotation3d(0.degrees, 0.degrees, 0.degrees) + ) + val CAMERA_NAMES = listOf("northstar_1", "northstar_2", "northstar_3") object Limelight { diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt index 8105f929..09ac97d3 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt @@ -299,7 +299,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB .pose3d ) - Logger.recordOutput("FieldFrameEstimator/odomTField", odomTField.transform2d) + Logger.recordOutput("FieldFrameEstimator/odomTField", doubleArrayOf(odomTField.translation.x.inMeters, odomTField.translation.y.inMeters, odomTField.rotation.inRadians)) Logger.recordOutput( "Odometry/targetPose", @@ -391,7 +391,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB undriftedSwerveDriveOdometry.update((gyroInputs.gyroYaw).inRotation2ds, undriftedModules) drift = undriftedPose.minus(odomTRobot) - Logger.recordOutput(VisionConstants.SIM_POSE_TOPIC_NAME, undriftedPose.pose2d) + Logger.recordOutput(VisionConstants.SIM_POSE_TOPIC_NAME, doubleArrayOf(undriftedPose.x.inMeters, undriftedPose.y.inMeters, undriftedPose.rotation.inRadians)) } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt index 147e95b6..e72442f6 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt @@ -2,6 +2,7 @@ package com.team4099.robot2023.subsystems.vision import com.team4099.lib.hal.Clock import com.team4099.lib.logging.TunableNumber +import com.team4099.lib.math.asDoubleArray import com.team4099.lib.vision.TimestampedVisionUpdate import com.team4099.robot2023.config.constants.FieldConstants import com.team4099.robot2023.config.constants.VisionConstants @@ -14,12 +15,14 @@ import org.team4099.lib.geometry.Pose2d import org.team4099.lib.geometry.Pose2dWPILIB import org.team4099.lib.geometry.Pose3d import org.team4099.lib.geometry.Pose3dWPILIB +import org.team4099.lib.geometry.Transform3d import org.team4099.lib.units.base.Time import org.team4099.lib.units.base.inMeters import org.team4099.lib.units.base.inMilliseconds import org.team4099.lib.units.base.meters import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRadians import java.util.function.Consumer import java.util.function.Supplier import kotlin.math.pow @@ -27,11 +30,12 @@ import kotlin.math.pow class Vision(vararg cameras: CameraIO) : SubsystemBase() { val io: List = cameras.toList() val inputs = List(io.size) { CameraIO.CameraInputs() } + val names = mutableListOf() + val robotTCameras = mutableListOf() companion object { val ambiguityThreshold = 0.7 val targetLogTime = 0.05.seconds - val cameraPoses = VisionConstants.CAMERA_TRANSFORMS val xyStdDevCoeffecient = 0.05 val thetaStdDevCoefficient = 1.5 @@ -49,6 +53,9 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { init { for (i in io.indices) { lastFrameTimes[i] = 0.0.seconds + names.add(io[i].id) + robotTCameras.add(io[i].robotTCamera) + } } @@ -75,7 +82,7 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { for (instance in io.indices) { io[instance].updateInputs(inputs[instance]) - Logger.processInputs("Vision/${VisionConstants.CAMERA_NAMES[instance]}", inputs[instance]) + Logger.processInputs("Vision/${names[instance]}", inputs[instance]) } var fieldTCurrentRobotEstimate: Pose2d = fieldFramePoseSupplier.get() @@ -90,7 +97,7 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { val values = inputs[instance].frame var cameraPose: Pose3d? = inputs[instance].frame - var robotPose: Pose2d? = cameraPose?.transformBy(cameraPoses[instance])?.toPose2d() + var robotPose: Pose2d? = cameraPose?.transformBy(robotTCameras[instance])?.toPose2d() if (cameraPose == null || robotPose == null) { @@ -125,30 +132,30 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { robotPoses.add(robotPose) Logger.recordOutput( - "Vision/${VisionConstants.CAMERA_NAMES[instance]}/latencyMS", + "Vision/${names[instance]}/latencyMS", (Clock.fpgaTime - timestamp).inMilliseconds ) Logger.recordOutput( - "Vision/${VisionConstants.CAMERA_NAMES[instance]}/estimatedRobotPose", robotPose.pose2d + "Vision/${names[instance]}/estimatedRobotPose", robotPose.asDoubleArray() ) Logger.recordOutput( - "Vision/${VisionConstants.CAMERA_NAMES[instance]}/tagPoses", + "Vision/${names[instance]}/tagPoses", *tagPoses.map { it.pose3d }.toTypedArray() ) - if (inputs[instance].timestamp == 0.0.seconds) { // prolly wrong lol - Logger.recordOutput( - "Vision/${VisionConstants.CAMERA_NAMES[instance]}/estimatedRobotPose", - Pose2dWPILIB.struct, - Pose2d().pose2d - ) - } +// if (inputs[instance].timestamp == 0.0.seconds) { // prolly wrong lol +// Logger.recordOutput( +// "Vision/${names[instance]}/estimatedRobotPose", +// Pose2dWPILIB.struct, +// Pose2d().pose2d +// ) +// } if (Clock.fpgaTime - lastFrameTimes[instance]!! > targetLogTime) { Logger.recordOutput( - "Vision/${VisionConstants.CAMERA_NAMES[instance]}/tagPoses", + "Vision/${names[instance]}/tagPoses", Pose3dWPILIB.struct, *arrayOf() ) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIO.kt index fb5f4264..ec8e6c99 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIO.kt @@ -5,10 +5,14 @@ import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.inputs.LoggableInputs import org.team4099.lib.geometry.Pose3d import org.team4099.lib.geometry.Pose3dWPILIB +import org.team4099.lib.geometry.Transform3d import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.base.seconds interface CameraIO { + + val id: String + val robotTCamera: Transform3d class CameraInputs : LoggableInputs { var timestamp = 0.0.seconds var frame: Pose3d = Pose3d() diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOPhotonvision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOPhotonvision.kt index e327a2b5..77f14d39 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOPhotonvision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOPhotonvision.kt @@ -6,6 +6,7 @@ import org.photonvision.PhotonCamera import org.photonvision.PhotonPoseEstimator import org.photonvision.PhotonPoseEstimator.PoseStrategy import org.team4099.lib.geometry.Pose3d +import org.team4099.lib.geometry.Transform3d import org.team4099.lib.geometry.Transform3dWPILIB import org.team4099.lib.units.base.Time import org.team4099.lib.units.base.inSeconds @@ -13,14 +14,14 @@ import org.team4099.lib.units.base.seconds import org.team4099.lib.units.micro import java.util.Optional -class CameraIOPhotonvision(private val identifier: String) : CameraIO { +class CameraIOPhotonvision(override val id: String, override val robotTCamera: Transform3d) : CameraIO { private val photonEstimator: PhotonPoseEstimator private val camera: PhotonCamera private var lastEstTimestamp: Time = 0.0.seconds init { - camera = PhotonCamera(identifier) + camera = PhotonCamera(id) photonEstimator = PhotonPoseEstimator( FieldConstants.wpilibFieldLayout, diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOSim.kt index 91dc86ed..d2c9cb8d 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOSim.kt @@ -12,13 +12,14 @@ import org.littletonrobotics.junction.Logger import org.team4099.lib.geometry.Pose2d import org.team4099.lib.geometry.Pose3d import org.team4099.lib.geometry.Rotation3d +import org.team4099.lib.geometry.Transform3d import org.team4099.lib.geometry.Translation2d import org.team4099.lib.geometry.Translation3d import org.team4099.lib.units.base.meters import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.radians -class CameraIOSim(val id: String) : CameraIO { +class CameraIOSim(override val id: String, override val robotTCamera: Transform3d) : CameraIO { private val poseSubscriber: DoubleArraySubscriber = NetworkTableInstance.getDefault() .getTable("AdvantageKit") @@ -32,7 +33,7 @@ class CameraIOSim(val id: String) : CameraIO { inputs.timestamp = Clock.fpgaTime - (Math.random() * 0.2).seconds val robotTtag = drivePose.relativeTo(FieldConstants.aprilTags[0].pose) - inputs.frame = robotTtag.gaussianShenanigans() + inputs.frame = drivePose.gaussianShenanigans() val estimatedRobotPose = FieldConstants.aprilTags[0].pose.transformBy(inputs.frame.toTransform3d()) From b7f086cb1d96a4246da383f4b11384a774514f00 Mon Sep 17 00:00:00 2001 From: Saraansh Wadkar Date: Sun, 18 Feb 2024 02:30:10 -0500 Subject: [PATCH 3/6] interpolating drive poses in sim --- .../subsystems/vision/camera/CameraIOSim.kt | 34 +++++++++++++++++-- 1 file changed, 31 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOSim.kt index d2c9cb8d..0f06e396 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOSim.kt @@ -15,6 +15,8 @@ import org.team4099.lib.geometry.Rotation3d import org.team4099.lib.geometry.Transform3d import org.team4099.lib.geometry.Translation2d import org.team4099.lib.geometry.Translation3d +import org.team4099.lib.units.base.Time +import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.base.meters import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.radians @@ -27,13 +29,31 @@ class CameraIOSim(override val id: String, override val robotTCamera: Transform3 .getDoubleArrayTopic(VisionConstants.POSE_TOPIC_NAME) .subscribe(DoubleArray(3)) private val errorScaleConstant = 0.05 + private val odoHistory = mutableListOf>() override fun updateInputs(inputs: CameraIO.CameraInputs) { val drivePose = poseSubscriber.get(DoubleArray(3)).toPose2d().toPose3d() + odoHistory.add(Pair(Clock.fpgaTime, drivePose.toPose2d())) - inputs.timestamp = Clock.fpgaTime - (Math.random() * 0.2).seconds - val robotTtag = drivePose.relativeTo(FieldConstants.aprilTags[0].pose) - inputs.frame = drivePose.gaussianShenanigans() + // clear out old poses history + if (odoHistory.size > 1 && odoHistory[0].first < Clock.fpgaTime - 0.3.seconds){ + odoHistory.removeAt(0) + } + + inputs.timestamp = Clock.fpgaTime - (Math.random() * 0.3).seconds + + var interpolatedDrivePose = Pose2d() + for (poseInd in 1 until odoHistory.size){ + // pose within these two timestamps + if (odoHistory[poseInd].first > inputs.timestamp && odoHistory[poseInd-1].first < inputs.timestamp){ + val interpolatingFrac = odoHistory[poseInd - 1].first.lerp(odoHistory[poseInd].first, inputs.timestamp) + interpolatedDrivePose = odoHistory[poseInd - 1].second.lerp(odoHistory[poseInd].second, interpolatingFrac) + } + } + + + val robotTtag = interpolatedDrivePose.toPose3d().relativeTo(FieldConstants.aprilTags[0].pose) + inputs.frame = interpolatedDrivePose.toPose3d().gaussianShenanigans() val estimatedRobotPose = FieldConstants.aprilTags[0].pose.transformBy(inputs.frame.toTransform3d()) @@ -66,4 +86,12 @@ class CameraIOSim(override val id: String, override val robotTCamera: Transform3 ) ) } + + private fun Pose2d.lerp(endValue: Pose2d, t: Double): Pose2d { + return this.plus(endValue.minus(this).times(t)) + } + + private fun Time.lerp(endTime: Time, desiredTime: Time): Double { + return (desiredTime - this).inSeconds / (endTime - this).inSeconds + } } From 2b5ce62ca015ef6c65ce497796146b408f390e97 Mon Sep 17 00:00:00 2001 From: Saraansh Wadkar Date: Sun, 18 Feb 2024 02:31:21 -0500 Subject: [PATCH 4/6] spotless apply --- .../kotlin/com/team4099/lib/math/GeomUtil.kt | 6 ++-- .../config/constants/VisionConstants.kt | 8 ++--- .../subsystems/drivetrain/drive/Drivetrain.kt | 16 +++++++-- .../robot2023/subsystems/vision/Vision.kt | 33 +++++++------------ .../vision/camera/CameraIOPhotonvision.kt | 3 +- .../subsystems/vision/camera/CameraIOSim.kt | 19 ++++++----- 6 files changed, 46 insertions(+), 39 deletions(-) diff --git a/src/main/kotlin/com/team4099/lib/math/GeomUtil.kt b/src/main/kotlin/com/team4099/lib/math/GeomUtil.kt index 5c0f3898..5972b837 100644 --- a/src/main/kotlin/com/team4099/lib/math/GeomUtil.kt +++ b/src/main/kotlin/com/team4099/lib/math/GeomUtil.kt @@ -51,9 +51,11 @@ fun Transform2d.asPose2d(): Pose2d { } fun Transform2d.asDoubleArray(): DoubleArray { - return doubleArrayOf(this.translation.x.inMeters, this.translation.y.inMeters, this.rotation.inRadians) + return doubleArrayOf( + this.translation.x.inMeters, this.translation.y.inMeters, this.rotation.inRadians + ) } fun Pose2d.asDoubleArray(): DoubleArray { return doubleArrayOf(this.x.inMeters, this.y.inMeters, this.rotation.inRadians) -} \ No newline at end of file +} diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt index ac5eca98..c85f5a7e 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt @@ -56,10 +56,10 @@ object VisionConstants { ) // camera facing leftward ) - val SIM_CAMERA_TRANSFORM = Transform3d( - Translation3d(0.inches, 0.inches, 0.inches), - Rotation3d(0.degrees, 0.degrees, 0.degrees) - ) + val SIM_CAMERA_TRANSFORM = + Transform3d( + Translation3d(0.inches, 0.inches, 0.inches), Rotation3d(0.degrees, 0.degrees, 0.degrees) + ) val CAMERA_NAMES = listOf("northstar_1", "northstar_2", "northstar_3") diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt index 09ac97d3..aaec8514 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt @@ -299,7 +299,14 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB .pose3d ) - Logger.recordOutput("FieldFrameEstimator/odomTField", doubleArrayOf(odomTField.translation.x.inMeters, odomTField.translation.y.inMeters, odomTField.rotation.inRadians)) + Logger.recordOutput( + "FieldFrameEstimator/odomTField", + doubleArrayOf( + odomTField.translation.x.inMeters, + odomTField.translation.y.inMeters, + odomTField.rotation.inRadians + ) + ) Logger.recordOutput( "Odometry/targetPose", @@ -391,7 +398,12 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB undriftedSwerveDriveOdometry.update((gyroInputs.gyroYaw).inRotation2ds, undriftedModules) drift = undriftedPose.minus(odomTRobot) - Logger.recordOutput(VisionConstants.SIM_POSE_TOPIC_NAME, doubleArrayOf(undriftedPose.x.inMeters, undriftedPose.y.inMeters, undriftedPose.rotation.inRadians)) + Logger.recordOutput( + VisionConstants.SIM_POSE_TOPIC_NAME, + doubleArrayOf( + undriftedPose.x.inMeters, undriftedPose.y.inMeters, undriftedPose.rotation.inRadians + ) + ) } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt index e72442f6..d81ef939 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt @@ -5,14 +5,12 @@ import com.team4099.lib.logging.TunableNumber import com.team4099.lib.math.asDoubleArray import com.team4099.lib.vision.TimestampedVisionUpdate import com.team4099.robot2023.config.constants.FieldConstants -import com.team4099.robot2023.config.constants.VisionConstants import com.team4099.robot2023.subsystems.vision.camera.CameraIO import edu.wpi.first.math.VecBuilder import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj2.command.SubsystemBase import org.littletonrobotics.junction.Logger import org.team4099.lib.geometry.Pose2d -import org.team4099.lib.geometry.Pose2dWPILIB import org.team4099.lib.geometry.Pose3d import org.team4099.lib.geometry.Pose3dWPILIB import org.team4099.lib.geometry.Transform3d @@ -22,7 +20,6 @@ import org.team4099.lib.units.base.inMilliseconds import org.team4099.lib.units.base.meters import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.degrees -import org.team4099.lib.units.derived.inRadians import java.util.function.Consumer import java.util.function.Supplier import kotlin.math.pow @@ -55,7 +52,6 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { lastFrameTimes[i] = 0.0.seconds names.add(io[i].id) robotTCameras.add(io[i].robotTCamera) - } } @@ -99,7 +95,6 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { var cameraPose: Pose3d? = inputs[instance].frame var robotPose: Pose2d? = cameraPose?.transformBy(robotTCameras[instance])?.toPose2d() - if (cameraPose == null || robotPose == null) { continue } @@ -132,32 +127,26 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() { robotPoses.add(robotPose) Logger.recordOutput( - "Vision/${names[instance]}/latencyMS", - (Clock.fpgaTime - timestamp).inMilliseconds + "Vision/${names[instance]}/latencyMS", (Clock.fpgaTime - timestamp).inMilliseconds ) - Logger.recordOutput( - "Vision/${names[instance]}/estimatedRobotPose", robotPose.asDoubleArray() - ) + Logger.recordOutput("Vision/${names[instance]}/estimatedRobotPose", robotPose.asDoubleArray()) Logger.recordOutput( - "Vision/${names[instance]}/tagPoses", - *tagPoses.map { it.pose3d }.toTypedArray() + "Vision/${names[instance]}/tagPoses", *tagPoses.map { it.pose3d }.toTypedArray() ) -// if (inputs[instance].timestamp == 0.0.seconds) { // prolly wrong lol -// Logger.recordOutput( -// "Vision/${names[instance]}/estimatedRobotPose", -// Pose2dWPILIB.struct, -// Pose2d().pose2d -// ) -// } + // if (inputs[instance].timestamp == 0.0.seconds) { // prolly wrong lol + // Logger.recordOutput( + // "Vision/${names[instance]}/estimatedRobotPose", + // Pose2dWPILIB.struct, + // Pose2d().pose2d + // ) + // } if (Clock.fpgaTime - lastFrameTimes[instance]!! > targetLogTime) { Logger.recordOutput( - "Vision/${names[instance]}/tagPoses", - Pose3dWPILIB.struct, - *arrayOf() + "Vision/${names[instance]}/tagPoses", Pose3dWPILIB.struct, *arrayOf() ) } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOPhotonvision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOPhotonvision.kt index 77f14d39..1040d62c 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOPhotonvision.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOPhotonvision.kt @@ -14,7 +14,8 @@ import org.team4099.lib.units.base.seconds import org.team4099.lib.units.micro import java.util.Optional -class CameraIOPhotonvision(override val id: String, override val robotTCamera: Transform3d) : CameraIO { +class CameraIOPhotonvision(override val id: String, override val robotTCamera: Transform3d) : + CameraIO { private val photonEstimator: PhotonPoseEstimator private val camera: PhotonCamera diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOSim.kt index 0f06e396..6b4af282 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOSim.kt @@ -36,22 +36,25 @@ class CameraIOSim(override val id: String, override val robotTCamera: Transform3 odoHistory.add(Pair(Clock.fpgaTime, drivePose.toPose2d())) // clear out old poses history - if (odoHistory.size > 1 && odoHistory[0].first < Clock.fpgaTime - 0.3.seconds){ + if (odoHistory.size > 1 && odoHistory[0].first < Clock.fpgaTime - 0.3.seconds) { odoHistory.removeAt(0) } inputs.timestamp = Clock.fpgaTime - (Math.random() * 0.3).seconds var interpolatedDrivePose = Pose2d() - for (poseInd in 1 until odoHistory.size){ - // pose within these two timestamps - if (odoHistory[poseInd].first > inputs.timestamp && odoHistory[poseInd-1].first < inputs.timestamp){ - val interpolatingFrac = odoHistory[poseInd - 1].first.lerp(odoHistory[poseInd].first, inputs.timestamp) - interpolatedDrivePose = odoHistory[poseInd - 1].second.lerp(odoHistory[poseInd].second, interpolatingFrac) - } + for (poseInd in 1 until odoHistory.size) { + // pose within these two timestamps + if (odoHistory[poseInd].first > inputs.timestamp && + odoHistory[poseInd - 1].first < inputs.timestamp + ) { + val interpolatingFrac = + odoHistory[poseInd - 1].first.lerp(odoHistory[poseInd].first, inputs.timestamp) + interpolatedDrivePose = + odoHistory[poseInd - 1].second.lerp(odoHistory[poseInd].second, interpolatingFrac) + } } - val robotTtag = interpolatedDrivePose.toPose3d().relativeTo(FieldConstants.aprilTags[0].pose) inputs.frame = interpolatedDrivePose.toPose3d().gaussianShenanigans() val estimatedRobotPose = From b0f55b5f1106023dca421bb1636671b7a5aa08d4 Mon Sep 17 00:00:00 2001 From: Saraansh Wadkar Date: Sun, 18 Feb 2024 03:18:27 -0500 Subject: [PATCH 5/6] lower gaussian error constant --- .../team4099/robot2023/subsystems/vision/camera/CameraIOSim.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOSim.kt index 6b4af282..34aa02dd 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOSim.kt @@ -28,7 +28,7 @@ class CameraIOSim(override val id: String, override val robotTCamera: Transform3 .getSubTable("RealOutputs") .getDoubleArrayTopic(VisionConstants.POSE_TOPIC_NAME) .subscribe(DoubleArray(3)) - private val errorScaleConstant = 0.05 + private val errorScaleConstant = 0.01 private val odoHistory = mutableListOf>() override fun updateInputs(inputs: CameraIO.CameraInputs) { From cee3a4be1f021cb5991d7cc02aa07a4511c8b019 Mon Sep 17 00:00:00 2001 From: Saraansh Wadkar Date: Sun, 18 Feb 2024 18:11:42 -0500 Subject: [PATCH 6/6] drive fixes --- .../com/team4099/robot2023/RobotContainer.kt | 20 +++-- .../FollowPathPlannerPathCommand.kt | 4 +- .../config/constants/GyroConstants.kt | 2 +- .../subsystems/drivetrain/drive/Drivetrain.kt | 3 +- .../drivetrain/gyro/GyroIOPigeon2.kt | 9 +- .../swervemodule/SwerveModuleIOTalon.kt | 82 +------------------ 6 files changed, 29 insertions(+), 91 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index dfe00707..1e66a131 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -28,13 +28,16 @@ import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO import com.team4099.robot2023.subsystems.superstructure.Request import com.team4099.robot2023.subsystems.superstructure.Superstructure import com.team4099.robot2023.subsystems.vision.Vision -import com.team4099.robot2023.subsystems.vision.camera.CameraIOPhotonvision +import com.team4099.robot2023.subsystems.vision.camera.CameraIO import com.team4099.robot2023.subsystems.vision.camera.CameraIOSim import com.team4099.robot2023.subsystems.wrist.Wrist import com.team4099.robot2023.subsystems.wrist.WristIO import com.team4099.robot2023.subsystems.wrist.WristIOSim import com.team4099.robot2023.util.driver.Ryan import edu.wpi.first.wpilibj.RobotBase +import org.team4099.lib.geometry.Rotation3d +import org.team4099.lib.geometry.Transform3d +import org.team4099.lib.geometry.Translation3d import org.team4099.lib.smoothDeadband import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees @@ -59,11 +62,16 @@ object RobotContainer { drivetrain = Drivetrain(GyroIOPigeon2, DrivetrainIOReal) vision = Vision( - // object: CameraIO {} + object : CameraIO { + override val id: String + get() = "skrt" + override val robotTCamera: Transform3d + get() = Transform3d(Translation3d(), Rotation3d()) + } // CameraIONorthstar("northstar"), - CameraIOPhotonvision("parakeet_1", VisionConstants.CAMERA_TRANSFORMS[0]), - CameraIOPhotonvision("parakeet_2", VisionConstants.CAMERA_TRANSFORMS[1]), - CameraIOPhotonvision("parakeet_3", VisionConstants.CAMERA_TRANSFORMS[2]), + // CameraIOPhotonvision("parakeet_1", VisionConstants.CAMERA_TRANSFORMS[0]), + // CameraIOPhotonvision("parakeet_2", VisionConstants.CAMERA_TRANSFORMS[1]), + // CameraIOPhotonvision("parakeet_3", VisionConstants.CAMERA_TRANSFORMS[2]), // CameraIONorthstar("right"), // CameraIONorthstar("backward") ) @@ -147,7 +155,7 @@ object RobotContainer { fun mapTeleopControls() { - ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain, toAngle = 180.degrees)) + ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain, toAngle = 0.0.degrees)) ControlBoard.runGroundIntake.whileTrue(superstructure.groundIntakeCommand()) ControlBoard.ejectGamePiece.whileTrue(superstructure.ejectGamePieceCommand()) ControlBoard.prepAmpScore.whileTrue(superstructure.prepAmpCommand()) diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt index bf58d0a0..ed0fe1db 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt @@ -182,6 +182,8 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla // Sampling the trajectory for a state that we're trying to target val stateFromTrajectory = generatedTrajectory.sample(trajCurTime.inSeconds) + lastSampledPose = Pose2d(stateFromTrajectory.targetHolonomicPose) + // Retrieves the last sampled pose, so we can keep our `atReference` variable updated Logger.recordOutput( "Odometry/targetPose", @@ -196,8 +198,6 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla ) Logger.recordOutput("Pathfollow/currentTheta", drivetrain.odomTRobot.rotation.inDegrees) - lastSampledPose = Pose2d(stateFromTrajectory.targetHolonomicPose) - val targetedChassisSpeeds = swerveDriveController.calculateRobotRelativeSpeeds( drivetrain.odomTRobot, stateFromTrajectory diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/GyroConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/GyroConstants.kt index a6ac9b3c..b77770a1 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/GyroConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/GyroConstants.kt @@ -5,5 +5,5 @@ import org.team4099.lib.units.derived.degrees object GyroConstants { val mountPitch = 0.0.degrees val mountYaw = 0.0.degrees - val mountRoll = 0.0.degrees + val mountRoll = 180.0.degrees } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt index aaec8514..24924691 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt @@ -568,12 +568,13 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB * @param toAngle Zeros the gyro to the value */ fun zeroGyroYaw(toAngle: Angle = 0.degrees) { + gyroIO.zeroGyroYaw(toAngle) // TODO(parth): This feels incorrect -- I think the first arg should be the gyro angle and the // undrifted pose should be updated to toAngle if (RobotBase.isSimulation()) { // NOTE(parth): The gyro itself should never need to be reset in-match on a real robot, the // odometry can be updated directly - gyroIO.zeroGyroYaw(toAngle) + undriftedSwerveDriveOdometry.resetPosition( toAngle.inRotation2ds, swerveModules.map { it.modulePosition }.toTypedArray(), diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIOPigeon2.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIOPigeon2.kt index bc7ce3e5..22843335 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIOPigeon2.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIOPigeon2.kt @@ -153,14 +153,17 @@ object GyroIOPigeon2 : GyroIO { } override fun zeroGyroYaw(toAngle: Angle) { - gyroYawOffset = toAngle - pigeon2.yaw.value.IEEErem(360.0).degrees + Logger.recordOutput("Gyro/gyroYaw", gyroYaw.inDegrees) + Logger.recordOutput("Gyro/toAngle", toAngle.inDegrees) + gyroYawOffset = toAngle - yawSignal.value.IEEErem(360.0).degrees + Logger.recordOutput("Gyro/gyroYawOffsetDegrees", gyroYawOffset.inDegrees) } override fun zeroGyroPitch(toAngle: Angle) { - gyroPitchOffset = toAngle - pigeon2.pitch.value.IEEErem(360.0).degrees + gyroPitchOffset = toAngle - pitchSignal.value.IEEErem(360.0).degrees } override fun zeroGyroRoll(toAngle: Angle) { - gyroRollOffset = toAngle - pigeon2.roll.value.IEEErem(360.0).degrees + gyroRollOffset = toAngle - rollSignal.value.IEEErem(360.0).degrees } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt index 074e74e4..9d895724 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt @@ -10,6 +10,7 @@ import com.ctre.phoenix6.controls.DutyCycleOut import com.ctre.phoenix6.controls.PositionDutyCycle import com.ctre.phoenix6.controls.VelocityDutyCycle import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.signals.InvertedValue import com.ctre.phoenix6.signals.NeutralModeValue import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.DrivetrainConstants @@ -82,82 +83,6 @@ class SwerveModuleIOTalon( } } - init { - driveFalcon.configurator.apply(TalonFXConfiguration()) - steeringFalcon.configurator.apply(TalonFXConfiguration()) - - driveFalcon.clearStickyFaults() - steeringFalcon.clearStickyFaults() - - steeringConfiguration.Slot0.kP = - steeringSensor.proportionalPositionGainToRawUnits(DrivetrainConstants.PID.STEERING_KP) - steeringConfiguration.Slot0.kI = - steeringSensor.integralPositionGainToRawUnits(DrivetrainConstants.PID.STEERING_KI) - steeringConfiguration.Slot0.kD = - steeringSensor.derivativePositionGainToRawUnits(DrivetrainConstants.PID.STEERING_KD) - steeringConfiguration.Slot0.kV = - steeringSensor.velocityFeedforwardToRawUnits(DrivetrainConstants.PID.STEERING_KFF) - steeringConfiguration.MotionMagic.MotionMagicCruiseVelocity = - steeringSensor.velocityToRawUnits(DrivetrainConstants.STEERING_VEL_MAX) - steeringConfiguration.MotionMagic.MotionMagicAcceleration = - steeringSensor.accelerationToRawUnits(DrivetrainConstants.STEERING_ACCEL_MAX) - steeringConfiguration.CurrentLimits.SupplyCurrentLimit = - DrivetrainConstants.STEERING_SUPPLY_CURRENT_LIMIT.inAmperes - steeringConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true - steeringConfiguration.ClosedLoopGeneral.ContinuousWrap = true - steeringConfiguration.Feedback.SensorToMechanismRatio = - 1 / DrivetrainConstants.STEERING_SENSOR_GEAR_RATIO - steeringConfiguration.MotorOutput.NeutralMode = - NeutralModeValue.Brake // change back to coast maybe? - steeringFalcon.inverted = true - steeringFalcon.configurator.apply(steeringConfiguration) - - driveConfiguration.Slot0.kP = - driveSensor.proportionalVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KP) - driveConfiguration.Slot0.kI = - driveSensor.integralVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KI) - driveConfiguration.Slot0.kD = - driveSensor.derivativeVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KD) - driveConfiguration.Slot0.kV = 0.05425 - // driveSensor.velocityFeedforwardToRawUnits(DrivetrainConstants.PID.DRIVE_KFF) - driveConfiguration.CurrentLimits.SupplyCurrentLimit = - DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT.inAmperes - driveConfiguration.CurrentLimits.SupplyCurrentThreshold = - DrivetrainConstants.DRIVE_THRESHOLD_CURRENT_LIMIT.inAmperes - driveConfiguration.CurrentLimits.SupplyTimeThreshold = - DrivetrainConstants.DRIVE_TRIGGER_THRESHOLD_TIME.inSeconds - driveConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true - driveConfiguration.CurrentLimits.StatorCurrentLimit = - DrivetrainConstants.DRIVE_STATOR_CURRENT_LIMIT.inAmperes - driveConfiguration.CurrentLimits.StatorCurrentLimitEnable = false // TODO tune - - driveConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake - driveFalcon.configurator.apply(driveConfiguration) - - MotorChecker.add( - "Drivetrain", - "Drive", - MotorCollection( - mutableListOf(Falcon500(driveFalcon, "$label Drive Motor")), - DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT, - 90.celsius, - DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT - 30.amps, - 110.celsius - ) - ) - - MotorChecker.add( - "Drivetrain", - "Steering", - MotorCollection( - mutableListOf(Falcon500(steeringFalcon, "$label Steering Motor")), - DrivetrainConstants.STEERING_SUPPLY_CURRENT_LIMIT, - 90.celsius, - DrivetrainConstants.STEERING_SUPPLY_CURRENT_LIMIT - 10.amps, - 110.celsius - ) - ) - } val driveStatorCurrentSignal: StatusSignal val driveSupplyCurrentSignal: StatusSignal val steeringStatorCurrentSignal: StatusSignal @@ -207,7 +132,7 @@ class SwerveModuleIOTalon( driveSensor.integralVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KI) driveConfiguration.Slot0.kD = driveSensor.derivativeVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KD) - driveConfiguration.Slot0.kV = 0.05425 + driveConfiguration.Slot0.kV = 0.1267939375649165 / 15 // kv too high rn // driveSensor.velocityFeedforwardToRawUnits(DrivetrainConstants.PID.DRIVE_KFF) driveConfiguration.CurrentLimits.SupplyCurrentLimit = DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT.inAmperes @@ -219,8 +144,8 @@ class SwerveModuleIOTalon( driveConfiguration.CurrentLimits.StatorCurrentLimit = DrivetrainConstants.DRIVE_STATOR_CURRENT_LIMIT.inAmperes driveConfiguration.CurrentLimits.StatorCurrentLimitEnable = false // TODO tune - driveConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Coast + driveConfiguration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive driveFalcon.configurator.apply(driveConfiguration) driveStatorCurrentSignal = driveFalcon.statorCurrent @@ -477,6 +402,7 @@ class SwerveModuleIOTalon( } else { motorOutputConfig.NeutralMode = NeutralModeValue.Coast } + motorOutputConfig.Inverted = InvertedValue.Clockwise_Positive driveFalcon.configurator.apply(motorOutputConfig) }