+ ) =
+ createSparkMax(
+ name = name + "_Side",
+ id = motorId,
+ enableBrakeMode = true,
+ inverted = inverted,
+ encCreator = QuadEncoder.creator(
+ wpiEnc,
+ DifferentialConstants.DRIVE_EXT_ENC_CPR,
+ DifferentialConstants.DRIVE_UPR,
+ DifferentialConstants.DRIVE_GEARING,
+ encInverted
+ ),
+
+ slaveSparks = followers,
+ currentLimit = DifferentialConstants.DRIVE_CURRENT_LIM
+ )
+
+ fun createDifferentialDrive(ahrs: AHRS): DifferentialDrive {
+ return DifferentialDrive(
+ leftLeader = makeSide(
+ "Left",
+ DifferentialConstants.DRIVE_MOTOR_L,
+ inverted = false,
+ encInverted = false,
+ wpiEnc = DifferentialConstants.DRIVE_ENC_LEFT,
+ followers = mapOf(
+ DifferentialConstants.DRIVE_MOTOR_L1 to false,
+ DifferentialConstants.DRIVE_MOTOR_L2 to false
+ )
+ ),
+ rightLeader = makeSide(
+ "Right",
+ DifferentialConstants.DRIVE_MOTOR_R,
+ inverted = true,
+ encInverted = true,
+ wpiEnc = DifferentialConstants.DRIVE_ENC_RIGHT,
+ followers = mapOf(
+ DifferentialConstants.DRIVE_MOTOR_R1 to false,
+ DifferentialConstants.DRIVE_MOTOR_R2 to false
+ )
+ ),
+ ahrs,
+ DifferentialConstants.DRIVE_FEED_FORWARD,
+ {
+ PIDController(
+ DifferentialConstants.DRIVE_KP,
+ DifferentialConstants.DRIVE_KI,
+ DifferentialConstants.DRIVE_KD
+ )
+ },
+ DifferentialConstants.TRACK_WIDTH
+ )
+ }
+
+// fun simOf(
+// drive: DifferentialDrive,
+// kV: Double,
+// kA: Double,
+// angleKV: Double,
+// angleKA: Double,
+// wheelRadius: Double
+// ): DifferentialSim {
+// val drivePlant = LinearSystemId.identifyDrivetrainSystem(
+// kV,
+// kA,
+// angleKV,
+// angleKA
+// )
+// val driveSim = DifferentialDrivetrainSim(
+// drivePlant,
+// DCMotor.getNEO(3),
+// DifferentialConstants.DRIVE_GEARING,
+// drive.trackwidth,
+// wheelRadius,
+// VecBuilder.fill(0.001, 0.001, 0.001, 0.1, 0.1, 0.005, 0.005)
+// )
+// return DifferentialSim(driveSim, drive.leftLeader, drive.rightLeader, drive.ahrs, drive.feedforward, drive.makeVelPID, drive.trackwidth)
+// }
+ }
+}
diff --git a/src/main/kotlin/frc/team449/control/differential/DifferentialOIs.kt b/src/main/kotlin/frc/team449/control/differential/DifferentialOIs.kt
new file mode 100644
index 0000000..6fe4957
--- /dev/null
+++ b/src/main/kotlin/frc/team449/control/differential/DifferentialOIs.kt
@@ -0,0 +1,140 @@
+package frc.team449.control.differential
+
+import edu.wpi.first.math.filter.SlewRateLimiter
+import edu.wpi.first.math.kinematics.ChassisSpeeds
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds
+import edu.wpi.first.wpilibj.drive.DifferentialDrive.WheelSpeeds
+import frc.team449.control.OI
+import frc.team449.robot2023.constants.RobotConstants
+
+/**
+ * Helper class to create OIs for a differential drivetrain (arcade, curvature,
+ * or tank)
+ */
+object DifferentialOIs {
+ /**
+ * Create an OI for arcade drive. One throttle controls forward-backward speed,
+ * the other; controls rotation.
+ *
+ * @param drive The drivetrain
+ * @param xThrottle Throttle to get forward-backward movement
+ * @param rotThrottle Throttle to get rotation
+ * @param xRamp Used for limiting forward-backward acceleration
+ * @param rotRamp Used for limiting rotational acceleration
+ */
+ fun createArcade(
+ drive: DifferentialDrive,
+ xThrottle: () -> Double,
+ rotThrottle: () -> Double,
+ xRamp: SlewRateLimiter,
+ rotRamp: SlewRateLimiter
+ ): OI = OI {
+ scaleAndApplyRamping(
+ edu.wpi.first.wpilibj.drive.DifferentialDrive.arcadeDriveIK(
+ xThrottle(),
+ rotThrottle(),
+ false
+ ),
+ drive.kinematics,
+ xRamp,
+ rotRamp
+ )
+ }
+
+ /**
+ * Create OI for curvature drive (drives like a car). One throttle controls
+ * forward-backward speed, like arcade, but the other controls curvature instead
+ * of rotation. Ramping is still applied to rotation instead of curvature.
+ *
+ * @param drive The drivetrain
+ * @param xThrottle Throttle to get forward-backward movement
+ * @param rotThrottle Throttle to get rotation
+ * @param xRamp Used for limiting forward-backward acceleration
+ * @param rotRamp Used for limiting rotational acceleration
+ * @param turnInPlace When this returns true, turns in place instead of turning
+ * like a car
+ */
+ fun createCurvature(
+ drive: DifferentialDrive,
+ xThrottle: () -> Double,
+ rotThrottle: () -> Double,
+ xRamp: SlewRateLimiter,
+ rotRamp: SlewRateLimiter,
+ turnInPlace: () -> Boolean
+ ): OI = OI {
+ scaleAndApplyRamping(
+ edu.wpi.first.wpilibj.drive.DifferentialDrive.curvatureDriveIK(
+ xThrottle(),
+ rotThrottle(),
+ turnInPlace()
+ ),
+ drive.kinematics,
+ xRamp,
+ rotRamp
+ )
+ }
+
+ /**
+ * Create an OI for tank drive. Each throttle controls one side of the drive
+ * separately. Each side is also ramped separately.
+ *
+ *
+ * Shame on you if you ever use this.
+ *
+ * @param drive The drivetrain
+ * @param leftThrottle Throttle to get forward-backward movement
+ * @param rightThrottle Throttle to get rotation
+ * @param leftRamp Used for limiting the left side's acceleration
+ * @param rightRamp Used for limiting the right side's acceleration
+ */
+ fun createTank(
+ drive: DifferentialDrive,
+ leftThrottle: () -> Double,
+ rightThrottle: () -> Double,
+ leftRamp: SlewRateLimiter,
+ rightRamp: SlewRateLimiter
+ ): OI = OI {
+ drive.kinematics.toChassisSpeeds(
+ DifferentialDriveWheelSpeeds(
+ leftRamp.calculate(leftThrottle() * RobotConstants.MAX_LINEAR_SPEED),
+ rightRamp.calculate(
+ rightThrottle() * RobotConstants.MAX_LINEAR_SPEED
+ )
+ )
+ )
+ }
+
+ /**
+ * Scales differential drive throttles from [-1, 1] to [-maxSpeed, maxSpeed], then
+ * applies ramping to give the final [ChassisSpeeds].
+ *
+ *
+ * Do note that although this is given a [DifferentialDriveWheelSpeeds]
+ * object, the ramping isn't applied to the left and right side but to the
+ * linear and rotational velocity using a [ChassisSpeeds] object.
+ *
+ * @param wheelThrottles The left and right wheel throttles
+ * @param kinematics Kinematics object used for turning differential drive wheel
+ * speeds to chassis speeds
+ * @param ramp Used for limiting linear/forward-back acceleration
+ * @param rotRamp Used for limiting rotational acceleration
+ */
+ private fun scaleAndApplyRamping(
+ wheelThrottles: WheelSpeeds,
+ kinematics: DifferentialDriveKinematics,
+ ramp: SlewRateLimiter,
+ rotRamp: SlewRateLimiter
+ ): ChassisSpeeds {
+ val leftVel = wheelThrottles.left * RobotConstants.MAX_LINEAR_SPEED
+ val rightVel = wheelThrottles.right * RobotConstants.MAX_LINEAR_SPEED
+ val chassisSpeeds = kinematics.toChassisSpeeds(
+ DifferentialDriveWheelSpeeds(leftVel, rightVel)
+ )
+ return ChassisSpeeds(
+ ramp.calculate(chassisSpeeds.vxMetersPerSecond),
+ 0.0,
+ rotRamp.calculate(chassisSpeeds.omegaRadiansPerSecond)
+ )
+ }
+}
diff --git a/src/main/kotlin/frc/team449/control/differential/DifferentialSim.kt b/src/main/kotlin/frc/team449/control/differential/DifferentialSim.kt
new file mode 100644
index 0000000..6687e6f
--- /dev/null
+++ b/src/main/kotlin/frc/team449/control/differential/DifferentialSim.kt
@@ -0,0 +1,66 @@
+package frc.team449.control.differential
+
+// import edu.wpi.first.math.controller.DifferentialDriveFeedforward
+// import edu.wpi.first.math.controller.PIDController
+// import edu.wpi.first.math.controller.SimpleMotorFeedforward
+// import edu.wpi.first.math.geometry.Pose2d
+// import edu.wpi.first.math.geometry.Rotation2d
+// import edu.wpi.first.wpilibj.Timer
+// import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim
+// import frc.team449.system.AHRS
+// import frc.team449.system.encoder.Encoder
+// import frc.team449.system.motor.WrappedMotor
+//
+// class DifferentialSim(
+// private val driveSim: DifferentialDrivetrainSim,
+// leftLeader: WrappedMotor,
+// rightLeader: WrappedMotor,
+// ahrs: AHRS,
+// private val feedForward: DifferentialDriveFeedforward,
+// makeVelPID: () -> PIDController,
+// trackwidth: Double
+// ) : DifferentialDrive(leftLeader, rightLeader, ahrs, feedForward, makeVelPID, trackwidth) {
+//
+// private val leftEncSim = Encoder.SimController(leftLeader.encoder)
+// private val rightEncSim = Encoder.SimController(rightLeader.encoder)
+//
+// private var offset: Rotation2d = Rotation2d()
+// private var lastTime = Timer.getFPGATimestamp()
+//
+// override fun periodic() {
+// val currTime = Timer.getFPGATimestamp()
+//
+// val dt = currTime - lastTime
+//
+// val leftVel = desiredWheelSpeeds.leftMetersPerSecond
+// val rightVel = desiredWheelSpeeds.rightMetersPerSecond
+//
+// val sideVoltages = feedForward.calculate()
+//
+// val leftVoltage = feedForward.calculate(leftVel) + leftPID.calculate(driveSim.leftVelocityMetersPerSecond, leftVel)
+// val rightVoltage = feedForward.calculate(rightVel) + rightPID.calculate(driveSim.rightVelocityMetersPerSecond, rightVel)
+//
+// leftEncSim.velocity = driveSim.leftVelocityMetersPerSecond
+// rightEncSim.velocity = driveSim.rightVelocityMetersPerSecond
+// leftEncSim.position = driveSim.leftPositionMeters
+// rightEncSim.position = driveSim.rightPositionMeters
+//
+// driveSim.setInputs(leftVoltage, rightVoltage)
+//
+// driveSim.update(dt)
+// this.poseEstimator.update(heading, this.leftEncSim.position, this.rightEncSim.position)
+// this.lastTime = currTime
+// }
+//
+// override var pose: Pose2d
+// get() = driveSim.pose
+// set(value) {
+// driveSim.pose = value
+// }
+//
+// override var heading: Rotation2d
+// get() = driveSim.heading.rotateBy(offset)
+// set(value) {
+// offset = value - driveSim.heading
+// }
+// }
diff --git a/src/main/kotlin/frc/team449/control/holonomic/HolonomicDrive.kt b/src/main/kotlin/frc/team449/control/holonomic/HolonomicDrive.kt
new file mode 100644
index 0000000..7856dda
--- /dev/null
+++ b/src/main/kotlin/frc/team449/control/holonomic/HolonomicDrive.kt
@@ -0,0 +1,11 @@
+package frc.team449.control.holonomic
+
+import frc.team449.control.DriveSubsystem
+
+interface HolonomicDrive : DriveSubsystem {
+ /** The max speed that the drivetrain can translate at, in meters per second. */
+ var maxLinearSpeed: Double
+
+ /** The max speed that the drivetrain can turn in place at, in radians per second. */
+ var maxRotSpeed: Double
+}
diff --git a/src/main/kotlin/frc/team449/control/holonomic/HolonomicOI.kt b/src/main/kotlin/frc/team449/control/holonomic/HolonomicOI.kt
new file mode 100644
index 0000000..11e3a03
--- /dev/null
+++ b/src/main/kotlin/frc/team449/control/holonomic/HolonomicOI.kt
@@ -0,0 +1,141 @@
+package frc.team449.control.holonomic
+
+import edu.wpi.first.math.MathUtil
+import edu.wpi.first.math.filter.SlewRateLimiter
+import edu.wpi.first.math.geometry.Rotation2d
+import edu.wpi.first.math.geometry.Translation2d
+import edu.wpi.first.math.kinematics.ChassisSpeeds
+import edu.wpi.first.util.sendable.Sendable
+import edu.wpi.first.util.sendable.SendableBuilder
+import edu.wpi.first.wpilibj.Timer
+import edu.wpi.first.wpilibj.XboxController
+import frc.team449.control.OI
+import frc.team449.robot2023.constants.RobotConstants
+import java.util.function.DoubleSupplier
+import kotlin.math.abs
+import kotlin.math.hypot
+
+/**
+ * Create an OI for controlling a holonomic drivetrain (probably swerve).
+ * The x and y axes on one joystick are used to control x and y velocity (m/s),
+ * while the x axis on another joystick is used to control rotational velocity (m/s).
+ *
The magnitude of the acceleration is clamped
+ *
Note that the joystick's X
+ * axis corresponds to the robot's/field's Y and vice versa
+ *
+ * @param drive The drivetrain this OI is controlling
+ * @param xThrottle The Y axis of the strafing joystick
+ * @param yThrottle The X axis of the strafing joystick
+ * @param rotThrottle The X axis of the rotating joystick
+ * @param rotRamp Used to ramp angular velocity
+ * @param maxAccel Max accel, used for ramping
+ * @param fieldOriented Whether the OI x and y translation should
+ * be relative to the field rather than relative to the robot. This better be true.
+ */
+class HolonomicOI(
+ private val drive: HolonomicDrive,
+ private val xThrottle: DoubleSupplier,
+ private val yThrottle: DoubleSupplier,
+ private val rotThrottle: DoubleSupplier,
+ private val rotRamp: SlewRateLimiter,
+ private val maxAccel: Double,
+ private val fieldOriented: () -> Boolean
+) : OI, Sendable {
+
+ /** Previous X velocity (scaled and clamped). */
+ private var prevX = 0.0
+
+ /** Previous Y velocity (scaled and clamped) */
+ private var prevY = 0.0
+
+ private var prevTime = Double.NaN
+
+ private var dx = 0.0
+ private var dy = 0.0
+ private var magAcc = 0.0
+ private var dt = 0.0
+ private var magAccClamped = 0.0
+
+ /**
+ * @return The [ChassisSpeeds] for the given x, y and
+ * rotation input from the joystick */
+ override fun get(): ChassisSpeeds {
+ val currTime = Timer.getFPGATimestamp()
+ if (this.prevTime.isNaN()) {
+ this.prevTime = currTime - 0.02
+ }
+ this.dt = currTime - prevTime
+ this.prevTime = currTime
+
+ val xScaled = xThrottle.asDouble * drive.maxLinearSpeed
+ val yScaled = yThrottle.asDouble * drive.maxLinearSpeed
+
+ // Clamp the acceleration
+ this.dx = xScaled - this.prevX
+ this.dy = yScaled - this.prevY
+ this.magAcc = hypot(dx / dt, dy / dt)
+ this.magAccClamped = MathUtil.clamp(magAcc, -this.maxAccel, this.maxAccel)
+
+ // Scale the change in x and y the same as the acceleration
+ val factor = if (magAcc == 0.0) 0.0 else magAccClamped / magAcc
+ val dxClamped = dx * factor
+ val dyClamped = dy * factor
+ val xClamped = prevX + dxClamped
+ val yClamped = prevY + dyClamped
+
+ this.prevX = xClamped
+ this.prevY = yClamped
+
+ val rotRaw = rotThrottle.asDouble
+ val rotScaled = rotRamp.calculate(rotRaw * drive.maxRotSpeed)
+
+ // translation velocity vector
+ val vel = Translation2d(xClamped, yClamped)
+
+ return if (this.fieldOriented()) {
+ /** Quick fix for the velocity skewing towards the direction of rotation
+ * by rotating it with offset proportional to how much we are rotating
+ **/
+ vel.rotateBy(Rotation2d(-rotScaled * dt / 2))
+ ChassisSpeeds.fromFieldRelativeSpeeds(
+ vel.x,
+ vel.y,
+ rotScaled,
+ drive.heading
+ )
+ } else {
+ ChassisSpeeds(
+ vel.x,
+ vel.y,
+ rotScaled
+ )
+ }
+ }
+
+ override fun initSendable(builder: SendableBuilder) {
+ builder.addDoubleProperty("currX", this.xThrottle::getAsDouble, null)
+ builder.addDoubleProperty("currY", this.yThrottle::getAsDouble, null)
+ builder.addDoubleProperty("prevX", { this.prevX }, null)
+ builder.addDoubleProperty("prevY", { this.prevY }, null)
+ builder.addDoubleProperty("dx", { this.dx }, null)
+ builder.addDoubleProperty("dy", { this.dy }, null)
+ builder.addDoubleProperty("dt", { this.dt }, null)
+ builder.addDoubleProperty("magAcc", { this.magAcc }, null)
+ builder.addDoubleProperty("magAccClamped", { this.magAccClamped }, null)
+ builder.addStringProperty("speeds", { this.get().toString() }, null)
+ }
+
+ companion object {
+ fun createHolonomicOI(drive: HolonomicDrive, driveController: XboxController): HolonomicOI {
+ return HolonomicOI(
+ drive,
+ { if (abs(driveController.leftY) < RobotConstants.TRANSLATION_DEADBAND) .0 else -driveController.leftY },
+ { if (abs(driveController.leftX) < RobotConstants.TRANSLATION_DEADBAND) .0 else -driveController.leftX },
+ { if (abs(driveController.getRawAxis(4)) < RobotConstants.ROTATION_DEADBAND) .0 else -driveController.getRawAxis(4) },
+ SlewRateLimiter(RobotConstants.ROT_RATE_LIMIT, RobotConstants.NEG_ROT_RATE_LIM, 0.0),
+ RobotConstants.MAX_ACCEL,
+ { true }
+ )
+ }
+ }
+}
diff --git a/src/main/kotlin/frc/team449/control/holonomic/MecanumDrive.kt b/src/main/kotlin/frc/team449/control/holonomic/MecanumDrive.kt
new file mode 100644
index 0000000..ea35e7c
--- /dev/null
+++ b/src/main/kotlin/frc/team449/control/holonomic/MecanumDrive.kt
@@ -0,0 +1,204 @@
+package frc.team449.control.holonomic
+
+import edu.wpi.first.math.MatBuilder
+import edu.wpi.first.math.Nat
+import edu.wpi.first.math.controller.PIDController
+import edu.wpi.first.math.controller.SimpleMotorFeedforward
+import edu.wpi.first.math.estimator.MecanumDrivePoseEstimator
+import edu.wpi.first.math.geometry.Pose2d
+import edu.wpi.first.math.geometry.Translation2d
+import edu.wpi.first.math.kinematics.ChassisSpeeds
+import edu.wpi.first.math.kinematics.MecanumDriveKinematics
+import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions
+import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds
+import edu.wpi.first.wpilibj.Timer
+import edu.wpi.first.wpilibj2.command.SubsystemBase
+import frc.team449.control.vision.VisionSubsystem
+import frc.team449.robot2023.constants.RobotConstants
+import frc.team449.robot2023.constants.drives.MecanumConstants
+import frc.team449.robot2023.constants.vision.VisionConstants
+import frc.team449.system.AHRS
+import frc.team449.system.encoder.NEOEncoder
+import frc.team449.system.motor.WrappedMotor
+import frc.team449.system.motor.createSparkMax
+
+/**
+ * @param frontLeftMotor the front left motor
+ * @param frontRightMotor the front right motor
+ * @param backLeftMotor the back left motor
+ * @param backRightMotor the back right motor
+ * @param frontLeftLocation the offset of the front left wheel to the center of the robot
+ * @param frontRightLocation the offset of the front right wheel to the center of the robot
+ * @param backLeftLocation the offset of the back left wheel to the center of the robot
+ * @param backRightLocation the offset of the back right wheel to the center of the robot
+ * @param maxLinearSpeed the maximum translation speed of the chassis.
+ * @param maxRotSpeed the maximum rotation speed of the chassis
+ * @param feedForward the SimpleMotorFeedforward for mecanum
+ * @param controller the PIDController for the robot
+ */
+open class MecanumDrive(
+ private val frontLeftMotor: WrappedMotor,
+ private val frontRightMotor: WrappedMotor,
+ private val backLeftMotor: WrappedMotor,
+ private val backRightMotor: WrappedMotor,
+ frontLeftLocation: Translation2d,
+ frontRightLocation: Translation2d,
+ backLeftLocation: Translation2d,
+ backRightLocation: Translation2d,
+ private val ahrs: AHRS,
+ override var maxLinearSpeed: Double,
+ override var maxRotSpeed: Double,
+ private val feedForward: SimpleMotorFeedforward,
+ private val controller: () -> PIDController,
+ private val cameras: List = mutableListOf()
+) : HolonomicDrive, SubsystemBase() {
+
+ private val flController = controller()
+ private val frController = controller()
+ private val blController = controller()
+ private val brController = controller()
+
+ private var lastTime = Timer.getFPGATimestamp()
+
+ val kinematics = MecanumDriveKinematics(
+ frontLeftLocation,
+ frontRightLocation,
+ backLeftLocation,
+ backRightLocation
+ )
+
+ private val poseEstimator = MecanumDrivePoseEstimator(
+ kinematics,
+ ahrs.heading,
+ getPositions(),
+ RobotConstants.INITIAL_POSE,
+ MatBuilder(Nat.N3(), Nat.N1()).fill(.005, .005, .0005), // [x, y, theta] other estimates
+ MatBuilder(Nat.N3(), Nat.N1()).fill(.005, .005, .0005) // [x, y, theta] vision estimates
+ )
+
+ override var pose: Pose2d
+ get() {
+ return this.poseEstimator.estimatedPosition
+ }
+ set(value) {
+ this.poseEstimator.resetPosition(ahrs.heading, getPositions(), value)
+ }
+
+ private var desiredWheelSpeeds = MecanumDriveWheelSpeeds()
+
+ override fun set(desiredSpeeds: ChassisSpeeds) {
+ desiredWheelSpeeds = kinematics.toWheelSpeeds(desiredSpeeds)
+ desiredWheelSpeeds.desaturate(MecanumConstants.MAX_ATTAINABLE_WHEEL_SPEED)
+ }
+
+ override fun stop() {
+ this.set(ChassisSpeeds(0.0, 0.0, 0.0))
+ }
+
+ override fun periodic() {
+ val currTime = Timer.getFPGATimestamp()
+
+ val frontLeftPID = flController.calculate(frontLeftMotor.velocity, desiredWheelSpeeds.frontLeftMetersPerSecond)
+ val frontRightPID = frController.calculate(frontRightMotor.velocity, desiredWheelSpeeds.frontRightMetersPerSecond)
+ val backLeftPID = blController.calculate(backLeftMotor.velocity, desiredWheelSpeeds.rearLeftMetersPerSecond)
+ val backRightPID = brController.calculate(backRightMotor.velocity, desiredWheelSpeeds.rearRightMetersPerSecond)
+
+ val frontLeftFF = feedForward.calculate(
+ desiredWheelSpeeds.frontLeftMetersPerSecond
+ )
+ val frontRightFF = feedForward.calculate(
+ desiredWheelSpeeds.frontRightMetersPerSecond
+ )
+ val backLeftFF = feedForward.calculate(
+ desiredWheelSpeeds.rearLeftMetersPerSecond
+ )
+ val backRightFF = feedForward.calculate(
+ desiredWheelSpeeds.rearRightMetersPerSecond
+ )
+
+ frontLeftMotor.setVoltage(frontLeftPID + frontLeftFF)
+ frontRightMotor.setVoltage(frontRightPID + frontRightFF)
+ backLeftMotor.setVoltage(backLeftPID + backLeftFF)
+ backRightMotor.setVoltage(backRightPID + backRightFF)
+
+ if (cameras.isNotEmpty()) localize()
+
+ this.poseEstimator.update(
+ ahrs.heading,
+ getPositions()
+ )
+
+ lastTime = currTime
+ }
+
+ /**
+ * @return the position readings of the wheels bundled into one object (meters)
+ */
+ private fun getPositions(): MecanumDriveWheelPositions =
+ MecanumDriveWheelPositions(
+ frontLeftMotor.position,
+ frontRightMotor.position,
+ backLeftMotor.position,
+ backRightMotor.position
+ )
+
+ /**
+ * @return the velocity readings of the wheels bundled into one object (meters/s)
+ */
+ private fun getSpeeds(): MecanumDriveWheelSpeeds =
+ MecanumDriveWheelSpeeds(
+ frontLeftMotor.velocity,
+ frontRightMotor.velocity,
+ backLeftMotor.velocity,
+ backRightMotor.velocity
+ )
+
+ private fun localize() {
+ for (camera in cameras) {
+ val result = camera.estimatedPose(Pose2d(pose.x, pose.y, ahrs.heading))
+ if (result.isPresent) {
+ poseEstimator.addVisionMeasurement(
+ result.get().estimatedPose.toPose2d(),
+ result.get().timestampSeconds
+ )
+ }
+ }
+ }
+
+ companion object {
+
+ /** Helper method to create a motor for each wheel */
+ private fun createCorner(name: String, motorID: Int, inverted: Boolean): WrappedMotor {
+ return createSparkMax(
+ name,
+ motorID,
+ NEOEncoder.creator(
+ MecanumConstants.DRIVE_UPR,
+ MecanumConstants.DRIVE_GEARING
+ ),
+ inverted = inverted,
+ currentLimit = MecanumConstants.CURRENT_LIM
+ )
+ }
+
+ /** Create a new Mecanum Drive from DriveConstants */
+ fun createMecanum(ahrs: AHRS): MecanumDrive {
+ return MecanumDrive(
+ createCorner("frontLeft", MecanumConstants.DRIVE_MOTOR_FL, false),
+ createCorner("frontRight", MecanumConstants.DRIVE_MOTOR_FR, true),
+ createCorner("backLeft", MecanumConstants.DRIVE_MOTOR_BL, false),
+ createCorner("backRight", MecanumConstants.DRIVE_MOTOR_BR, true),
+ Translation2d(MecanumConstants.WHEELBASE / 2, MecanumConstants.TRACKWIDTH / 2),
+ Translation2d(MecanumConstants.WHEELBASE / 2, -MecanumConstants.TRACKWIDTH / 2),
+ Translation2d(-MecanumConstants.WHEELBASE / 2, MecanumConstants.TRACKWIDTH / 2),
+ Translation2d(-MecanumConstants.WHEELBASE / 2, -MecanumConstants.TRACKWIDTH / 2),
+ ahrs,
+ RobotConstants.MAX_LINEAR_SPEED,
+ RobotConstants.MAX_ROT_SPEED,
+ SimpleMotorFeedforward(MecanumConstants.DRIVE_KS, MecanumConstants.DRIVE_KV, MecanumConstants.DRIVE_KA),
+ { PIDController(MecanumConstants.DRIVE_KP, MecanumConstants.DRIVE_KI, MecanumConstants.DRIVE_KD) },
+ VisionConstants.ESTIMATORS
+ )
+ }
+ }
+}
diff --git a/src/main/kotlin/frc/team449/control/holonomic/SwerveDrive.kt b/src/main/kotlin/frc/team449/control/holonomic/SwerveDrive.kt
new file mode 100644
index 0000000..0ea3208
--- /dev/null
+++ b/src/main/kotlin/frc/team449/control/holonomic/SwerveDrive.kt
@@ -0,0 +1,482 @@
+package frc.team449.control.holonomic
+
+import edu.wpi.first.math.MathUtil
+import edu.wpi.first.math.controller.PIDController
+import edu.wpi.first.math.controller.SimpleMotorFeedforward
+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.kinematics.ChassisSpeeds
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics
+import edu.wpi.first.math.kinematics.SwerveModulePosition
+import edu.wpi.first.math.kinematics.SwerveModuleState
+import edu.wpi.first.util.sendable.SendableBuilder
+import edu.wpi.first.wpilibj.DriverStation
+import edu.wpi.first.wpilibj.RobotBase.isReal
+import edu.wpi.first.wpilibj.smartdashboard.Field2d
+import edu.wpi.first.wpilibj2.command.SubsystemBase
+import frc.team449.control.vision.VisionSubsystem
+import frc.team449.robot2023.constants.RobotConstants
+import frc.team449.robot2023.constants.drives.SwerveConstants
+import frc.team449.robot2023.constants.vision.VisionConstants
+import frc.team449.system.AHRS
+import frc.team449.system.encoder.AbsoluteEncoder
+import frc.team449.system.encoder.NEOEncoder
+import frc.team449.system.motor.createSparkMax
+import kotlin.math.*
+
+/**
+ * A Swerve Drive chassis.
+ * @param modules An array of [SwerveModule]s that are on the drivetrain.
+ * @param ahrs The gyro that is mounted on the chassis.
+ * @param maxLinearSpeed The maximum translation speed of the chassis.
+ * @param maxRotSpeed The maximum rotation speed of the chassis.
+ * @param cameras The cameras that help estimate the robot's pose.
+ * @param field The SmartDashboard [Field2d] widget that shows the robot's pose.
+ */
+open class SwerveDrive(
+ protected val modules: List,
+ protected val ahrs: AHRS,
+ override var maxLinearSpeed: Double,
+ override var maxRotSpeed: Double,
+ protected val cameras: List = mutableListOf(),
+ protected val field: Field2d
+) : SubsystemBase(), HolonomicDrive {
+
+ /** Vision statistics */
+ protected var numTargets = DoubleArray(cameras.size)
+ protected var tagDistance = DoubleArray(cameras.size)
+ protected var avgAmbiguity = DoubleArray(cameras.size)
+ protected var heightError = DoubleArray(cameras.size)
+ protected var usedVision = BooleanArray(cameras.size)
+
+ /** The kinematics that convert [ChassisSpeeds] into multiple [SwerveModuleState] objects. */
+ protected val kinematics = SwerveDriveKinematics(
+ *this.modules.map { it.location }.toTypedArray()
+ )
+
+ /** The current speed of the robot's drive. */
+ var currentSpeeds = ChassisSpeeds()
+
+ /** Current estimated vision pose */
+ var visionPose = Pose2d()
+
+ /** Pose estimator that estimates the robot's position as a [Pose2d]. */
+ protected val poseEstimator = SwerveDrivePoseEstimator(
+ kinematics,
+ ahrs.heading,
+ getPositions(),
+ RobotConstants.INITIAL_POSE,
+ VisionConstants.ENCODER_TRUST,
+ VisionConstants.MULTI_TAG_TRUST
+ )
+
+ var desiredSpeeds: ChassisSpeeds = ChassisSpeeds()
+
+ protected var maxSpeed: Double = 0.0
+
+ override fun set(desiredSpeeds: ChassisSpeeds) {
+ this.desiredSpeeds = desiredSpeeds
+
+ // Converts the desired [ChassisSpeeds] into an array of [SwerveModuleState].
+ val desiredModuleStates =
+ this.kinematics.toSwerveModuleStates(this.desiredSpeeds)
+
+ // Scale down module speed if a module is going faster than the max speed, and prevent early desaturation.
+ normalizeDrive(desiredModuleStates, desiredSpeeds)
+// SwerveDriveKinematics.desaturateWheelSpeeds(
+// desiredModuleStates,
+// SwerveConstants.MAX_ATTAINABLE_MK4I_SPEED
+// )
+
+ for (i in this.modules.indices) {
+ this.modules[i].state = desiredModuleStates[i]
+ }
+
+ for (module in modules)
+ module.update()
+ }
+
+ // TODO: Do you notice a difference with this normalize function?
+ private fun normalizeDrive(desiredStates: Array, speeds: ChassisSpeeds) {
+ val translationalK: Double = hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond) / RobotConstants.MAX_LINEAR_SPEED
+ val rotationalK: Double = abs(speeds.omegaRadiansPerSecond) / RobotConstants.MAX_ROT_SPEED
+ val k = max(translationalK, rotationalK)
+
+ // Find how fast the fastest spinning drive motor is spinning
+ var realMaxSpeed = 0.0
+ for (moduleState in desiredStates) {
+ realMaxSpeed = max(realMaxSpeed, abs(moduleState.speedMetersPerSecond))
+ }
+
+ val scale =
+ if (realMaxSpeed > 0 && k < 1)
+ k * SwerveConstants.MAX_ATTAINABLE_MK4I_SPEED / realMaxSpeed
+ else if (realMaxSpeed > 0)
+ SwerveConstants.MAX_ATTAINABLE_MK4I_SPEED / realMaxSpeed
+ else
+ 1.0
+
+ for (moduleState in desiredStates) {
+ moduleState.speedMetersPerSecond *= scale
+ }
+ }
+
+ fun setVoltage(volts: Double) {
+ modules.forEach {
+ it.setVoltage(volts)
+ }
+ }
+
+ fun getModuleVel(): Double {
+ var totalVel = 0.0
+ modules.forEach { totalVel += it.state.speedMetersPerSecond }
+ return totalVel / modules.size
+ }
+
+ /** The measured pitch of the robot from the gyro sensor. */
+ val pitch: Rotation2d
+ get() = Rotation2d(MathUtil.angleModulus(ahrs.pitch.radians))
+
+ /** The measured roll of the robot from the gyro sensor. */
+ val roll: Rotation2d
+ get() = Rotation2d(MathUtil.angleModulus(ahrs.roll.radians))
+
+ /** The (x, y, theta) position of the robot on the field. */
+ override var pose: Pose2d
+ get() = this.poseEstimator.estimatedPosition
+ set(value) {
+ this.poseEstimator.resetPosition(
+ ahrs.heading,
+ getPositions(),
+ value
+ )
+ }
+
+ override fun periodic() {
+ // Updates the robot's currentSpeeds.
+ currentSpeeds = kinematics.toChassisSpeeds(
+ modules[0].state,
+ modules[1].state,
+ modules[2].state,
+ modules[3].state
+ )
+
+ val transVel = hypot(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond)
+ if (transVel > maxSpeed) maxSpeed = transVel
+
+ // Update the robot's pose using the gyro heading and the SwerveModulePositions of each module.
+ this.poseEstimator.update(
+ ahrs.heading,
+ getPositions()
+ )
+
+ if (cameras.isNotEmpty()) localize()
+
+ // Sets the robot's pose and individual module rotations on the SmartDashboard [Field2d] widget.
+ setRobotPose()
+ }
+
+ /** Stops the robot's drive. */
+ override fun stop() {
+ this.set(ChassisSpeeds(0.0, 0.0, 0.0))
+ }
+
+ /** @return An array of [SwerveModulePosition] for each module, containing distance and angle. */
+ protected fun getPositions(): Array {
+ return Array(modules.size) { i -> modules[i].position }
+ }
+
+ /** @return An array of [SwerveModuleState] for each module, containing speed and angle. */
+ private fun getStates(): Array {
+ return Array(modules.size) { i -> modules[i].state }
+ }
+
+ protected fun setRobotPose() {
+ this.field.robotPose = this.pose
+ this.field.getObject("FL").pose = this.pose.plus(Transform2d(Translation2d(SwerveConstants.WHEELBASE / 2, SwerveConstants.TRACKWIDTH / 2), this.getPositions()[0].angle))
+ this.field.getObject("FR").pose = this.pose.plus(Transform2d(Translation2d(SwerveConstants.WHEELBASE / 2, -SwerveConstants.TRACKWIDTH / 2), this.getPositions()[1].angle))
+ this.field.getObject("BL").pose = this.pose.plus(Transform2d(Translation2d(-SwerveConstants.WHEELBASE / 2, SwerveConstants.TRACKWIDTH / 2), this.getPositions()[2].angle))
+ this.field.getObject("BR").pose = this.pose.plus(Transform2d(Translation2d(-SwerveConstants.WHEELBASE / 2, -SwerveConstants.TRACKWIDTH / 2), this.getPositions()[0].angle))
+ }
+
+ protected open fun localize() = try {
+ for ((index, camera) in cameras.withIndex()) {
+ val result = camera.estimatedPose(Pose2d(pose.x, pose.y, ahrs.heading))
+ if (result.isPresent) {
+ val presentResult = result.get()
+ numTargets[index] = presentResult.targetsUsed.size.toDouble()
+ tagDistance[index] = 0.0
+ avgAmbiguity[index] = 0.0
+ heightError[index] = abs(presentResult.estimatedPose.z - camera.robotToCam.z)
+
+ for (tag in presentResult.targetsUsed) {
+ val tagPose = camera.estimator.fieldTags.getTagPose(tag.fiducialId)
+ if (tagPose.isPresent) {
+ val estimatedToTag = presentResult.estimatedPose.minus(tagPose.get())
+ tagDistance[index] += sqrt(estimatedToTag.x.pow(2) + estimatedToTag.y.pow(2)) / numTargets[index]
+ avgAmbiguity[index] = tag.poseAmbiguity / numTargets[index]
+ } else {
+ tagDistance[index] = Double.MAX_VALUE
+ avgAmbiguity[index] = Double.MAX_VALUE
+ break
+ }
+ }
+
+ visionPose = presentResult.estimatedPose.toPose2d()
+
+ if (presentResult.timestampSeconds > 0 &&
+ avgAmbiguity[index] <= VisionConstants.MAX_AMBIGUITY &&
+ numTargets[index] < 2 && tagDistance[index] <= VisionConstants.MAX_DISTANCE_SINGLE_TAG ||
+ numTargets[index] >= 2 && tagDistance[index] <= VisionConstants.MAX_DISTANCE_MULTI_TAG * (1 + (numTargets[index] - 2) * VisionConstants.TAG_MULTIPLIER) &&
+ heightError[index] < VisionConstants.MAX_HEIGHT_ERR_METERS
+ ) {
+ poseEstimator.addVisionMeasurement(
+ visionPose,
+ presentResult.timestampSeconds,
+ camera.getEstimationStdDevs(numTargets[index].toInt(), tagDistance[index])
+ )
+ usedVision[index] = true
+ } else {
+ usedVision[index] = false
+ }
+ }
+ }
+ } catch (e: Error) {
+ DriverStation.reportError(
+ "!!!!!!!!! VISION ERROR !!!!!!!",
+ e.stackTrace
+ )
+ }
+
+ override fun initSendable(builder: SendableBuilder) {
+ builder.publishConstString("1.0", "Poses and ChassisSpeeds")
+ builder.addDoubleArrayProperty("1.1 Estimated Pose", { doubleArrayOf(pose.x, pose.y, pose.rotation.radians) }, null)
+ builder.addDoubleArrayProperty("1.2 Vision Pose", { doubleArrayOf(visionPose.x, visionPose.y, visionPose.rotation.radians) }, null)
+ builder.addDoubleArrayProperty("1.3 Current Chassis Speeds", { doubleArrayOf(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond, currentSpeeds.omegaRadiansPerSecond) }, null)
+ builder.addDoubleArrayProperty("1.4 Desired Chassis Speeds", { doubleArrayOf(desiredSpeeds.vxMetersPerSecond, desiredSpeeds.vyMetersPerSecond, desiredSpeeds.omegaRadiansPerSecond) }, null)
+ builder.addDoubleProperty("1.5 Max Recorded Speed", { maxSpeed }, null)
+
+ builder.publishConstString("2.0", "Vision Stats")
+ builder.addBooleanArrayProperty("2.1 Used Last Vision Estimate?", { usedVision }, null)
+ builder.addDoubleArrayProperty("2.2 Number of Targets", { numTargets }, null)
+ builder.addDoubleArrayProperty("2.3 Avg Tag Distance", { tagDistance }, null)
+ builder.addDoubleArrayProperty("2.4 Average Ambiguity", { avgAmbiguity }, null)
+ builder.addDoubleArrayProperty("2.5 Cam Height Error", { heightError }, null)
+
+ builder.publishConstString("3.0", "Steering Rot (Std Order FL, FR, BL, BR)")
+ builder.addDoubleArrayProperty(
+ "3.1 Current Rotation",
+ { DoubleArray(modules.size) { index -> modules[index].position.angle.rotations } },
+ null
+ )
+ builder.addDoubleArrayProperty(
+ "3.2 Desired Rotation",
+ { DoubleArray(modules.size) { index -> modules[index].desiredState.angle.rotations } },
+ null
+ )
+
+ builder.publishConstString("4.0", "Module Driving Speeds (Std Order FL, FR, BL, BR)")
+ builder.addDoubleArrayProperty(
+ "4.1 Desired Speed",
+ { DoubleArray(modules.size) { index -> modules[index].desiredState.speedMetersPerSecond } },
+ null
+ )
+ builder.addDoubleArrayProperty(
+ "4.2 Current Speeds",
+ { DoubleArray(modules.size) { index -> modules[index].state.speedMetersPerSecond } },
+ null
+ )
+
+ builder.publishConstString("5.0", "Last Module Voltages (Standard Order, FL, FR, BL, BR)")
+ builder.addDoubleArrayProperty(
+ "5.1 Driving",
+ { DoubleArray(modules.size) { index -> modules[index].lastDrivingVoltage() } },
+ null
+ )
+ builder.addDoubleArrayProperty(
+ "5.2 Steering",
+ { DoubleArray(modules.size) { index -> modules[index].lastSteeringVoltage() } },
+ null
+ )
+
+ builder.publishConstString("6.0", "AHRS Values")
+ builder.addDoubleProperty("6.1 Heading Degrees", { ahrs.heading.degrees }, null)
+ builder.addDoubleProperty("6.2 Pitch Degrees", { ahrs.pitch.degrees }, null)
+ builder.addDoubleProperty("6.3 Roll Degrees", { ahrs.roll.degrees }, null)
+ builder.addDoubleProperty("6.4 Angular X Vel", { ahrs.angularXVel() }, null)
+
+ // Note: You should also tune UPR too
+ builder.publishConstString("7.0", "Tuning Values")
+ builder.addDoubleProperty("7.1 FL Drive P", { modules[0].driveController.p }, { value -> modules[0].driveController.p = value })
+ builder.addDoubleProperty("7.2 FL Drive D", { modules[0].driveController.d }, { value -> modules[0].driveController.d = value })
+ builder.addDoubleProperty("7.3 FL Turn P", { modules[0].turnController.p }, { value -> modules[0].turnController.p = value })
+ builder.addDoubleProperty("7.4 FL Turn D", { modules[0].turnController.d }, { value -> modules[0].turnController.d = value })
+ builder.addDoubleProperty("7.5 FR Drive P", { modules[1].driveController.p }, { value -> modules[1].driveController.p = value })
+ builder.addDoubleProperty("7.6 FR Drive D", { modules[1].driveController.d }, { value -> modules[1].driveController.d = value })
+ builder.addDoubleProperty("7.8 FR Turn P", { modules[1].turnController.p }, { value -> modules[1].turnController.p = value })
+ builder.addDoubleProperty("7.9 FR Turn D", { modules[1].turnController.d }, { value -> modules[1].turnController.d = value })
+ builder.addDoubleProperty("7.10 BL Drive P", { modules[2].driveController.p }, { value -> modules[2].driveController.p = value })
+ builder.addDoubleProperty("7.11 BL Drive D", { modules[2].driveController.d }, { value -> modules[2].driveController.d = value })
+ builder.addDoubleProperty("7.12 BL Turn P", { modules[2].turnController.p }, { value -> modules[2].turnController.p = value })
+ builder.addDoubleProperty("7.13 BL Turn D", { modules[2].turnController.d }, { value -> modules[2].turnController.d = value })
+ builder.addDoubleProperty("7.14 BR Drive P", { modules[3].driveController.p }, { value -> modules[3].driveController.p = value })
+ builder.addDoubleProperty("7.15 BR Drive D", { modules[3].driveController.d }, { value -> modules[3].driveController.d = value })
+ builder.addDoubleProperty("7.16 BR Turn P", { modules[3].turnController.p }, { value -> modules[3].turnController.p = value })
+ builder.addDoubleProperty("7.17 BR Turn D", { modules[3].turnController.d }, { value -> modules[3].turnController.d = value })
+ }
+
+ companion object {
+ /** Create a [SwerveDrive] using [SwerveConstants]. */
+ fun createSwerve(ahrs: AHRS, field: Field2d): SwerveDrive {
+ val driveMotorController = { PIDController(SwerveConstants.DRIVE_KP, SwerveConstants.DRIVE_KI, SwerveConstants.DRIVE_KD) }
+ val turnMotorController = { PIDController(SwerveConstants.TURN_KP, SwerveConstants.TURN_KI, SwerveConstants.TURN_KD) }
+ val driveFeedforward = SimpleMotorFeedforward(SwerveConstants.DRIVE_KS, SwerveConstants.DRIVE_KV, SwerveConstants.DRIVE_KA)
+ val modules = listOf(
+ SwerveModule.create(
+ "FLModule",
+ makeDrivingMotor(
+ "FL",
+ SwerveConstants.DRIVE_MOTOR_FL,
+ inverted = false
+ ),
+ makeTurningMotor(
+ "FL",
+ SwerveConstants.TURN_MOTOR_FL,
+ inverted = true,
+ sensorPhase = false,
+ SwerveConstants.TURN_ENC_CHAN_FL,
+ SwerveConstants.TURN_ENC_OFFSET_FL
+ ),
+ driveMotorController(),
+ turnMotorController(),
+ driveFeedforward,
+ Translation2d(SwerveConstants.WHEELBASE / 2, SwerveConstants.TRACKWIDTH / 2)
+ ),
+ SwerveModule.create(
+ "FRModule",
+ makeDrivingMotor(
+ "FR",
+ SwerveConstants.DRIVE_MOTOR_FR,
+ inverted = false
+ ),
+ makeTurningMotor(
+ "FR",
+ SwerveConstants.TURN_MOTOR_FR,
+ inverted = true,
+ sensorPhase = false,
+ SwerveConstants.TURN_ENC_CHAN_FR,
+ SwerveConstants.TURN_ENC_OFFSET_FR
+ ),
+ driveMotorController(),
+ turnMotorController(),
+ driveFeedforward,
+ Translation2d(SwerveConstants.WHEELBASE / 2, -SwerveConstants.TRACKWIDTH / 2)
+ ),
+ SwerveModule.create(
+ "BLModule",
+ makeDrivingMotor(
+ "BL",
+ SwerveConstants.DRIVE_MOTOR_BL,
+ inverted = false
+ ),
+ makeTurningMotor(
+ "BL",
+ SwerveConstants.TURN_MOTOR_BL,
+ inverted = true,
+ sensorPhase = false,
+ SwerveConstants.TURN_ENC_CHAN_BL,
+ SwerveConstants.TURN_ENC_OFFSET_BL
+ ),
+ driveMotorController(),
+ turnMotorController(),
+ driveFeedforward,
+ Translation2d(-SwerveConstants.WHEELBASE / 2, SwerveConstants.TRACKWIDTH / 2)
+ ),
+ SwerveModule.create(
+ "BRModule",
+ makeDrivingMotor(
+ "BR",
+ SwerveConstants.DRIVE_MOTOR_BR,
+ inverted = false
+ ),
+ makeTurningMotor(
+ "BR",
+ SwerveConstants.TURN_MOTOR_BR,
+ inverted = true,
+ sensorPhase = false,
+ SwerveConstants.TURN_ENC_CHAN_BR,
+ SwerveConstants.TURN_ENC_OFFSET_BR
+ ),
+ driveMotorController(),
+ turnMotorController(),
+ driveFeedforward,
+ Translation2d(-SwerveConstants.WHEELBASE / 2, -SwerveConstants.TRACKWIDTH / 2)
+ )
+ )
+ return if (isReal()) {
+ SwerveDrive(
+ modules,
+ ahrs,
+ RobotConstants.MAX_LINEAR_SPEED,
+ RobotConstants.MAX_ROT_SPEED,
+ VisionConstants.ESTIMATORS,
+ field
+ )
+ } else {
+ SwerveSim(
+ modules,
+ ahrs,
+ RobotConstants.MAX_LINEAR_SPEED,
+ RobotConstants.MAX_ROT_SPEED,
+ VisionConstants.ESTIMATORS,
+ field
+ )
+ }
+ }
+
+ /** Helper to make turning motors for swerve. */
+ private fun makeDrivingMotor(
+ name: String,
+ motorId: Int,
+ inverted: Boolean
+ ) =
+ createSparkMax(
+ name = name + "Drive",
+ id = motorId,
+ enableBrakeMode = true,
+ inverted = inverted,
+ encCreator =
+ NEOEncoder.creator(
+ SwerveConstants.DRIVE_UPR,
+ SwerveConstants.DRIVE_GEARING
+ ),
+ currentLimit = SwerveConstants.DRIVE_CURRENT_LIM
+ )
+
+ /** Helper to make turning motors for swerve. */
+ private fun makeTurningMotor(
+ name: String,
+ motorId: Int,
+ inverted: Boolean,
+ sensorPhase: Boolean,
+ encoderChannel: Int,
+ offset: Double
+ ) =
+ createSparkMax(
+ name = name + "Turn",
+ id = motorId,
+ enableBrakeMode = false,
+ inverted = inverted,
+ encCreator = AbsoluteEncoder.creator(
+ encoderChannel,
+ offset,
+ SwerveConstants.TURN_UPR,
+ sensorPhase
+ ),
+ currentLimit = SwerveConstants.STEERING_CURRENT_LIM
+ )
+ }
+}
diff --git a/src/main/kotlin/frc/team449/control/holonomic/SwerveModule.kt b/src/main/kotlin/frc/team449/control/holonomic/SwerveModule.kt
new file mode 100644
index 0000000..82a7578
--- /dev/null
+++ b/src/main/kotlin/frc/team449/control/holonomic/SwerveModule.kt
@@ -0,0 +1,206 @@
+package frc.team449.control.holonomic
+
+import edu.wpi.first.math.controller.PIDController
+import edu.wpi.first.math.controller.SimpleMotorFeedforward
+import edu.wpi.first.math.geometry.Rotation2d
+import edu.wpi.first.math.geometry.Translation2d
+import edu.wpi.first.math.kinematics.SwerveModulePosition
+import edu.wpi.first.math.kinematics.SwerveModuleState
+import edu.wpi.first.wpilibj.RobotBase
+import edu.wpi.first.wpilibj.Timer
+import frc.team449.robot2023.constants.drives.SwerveConstants
+import frc.team449.system.encoder.Encoder
+import frc.team449.system.motor.WrappedMotor
+import kotlin.math.PI
+import kotlin.math.abs
+import kotlin.math.sign
+
+/**
+ * Controls a Swerve Module.
+ * @param name The name of the module (used for logging).
+ * @param drivingMotor The motor that controls the speed of the module.
+ * @param turningMotor The motor that controls the angle of the module
+ * @param driveController The velocity control for speed of the module
+ * @param turnController The position control for the angle of the module
+ * @param driveFeedforward The voltage predicting equation for a given speed of the module.
+ * @param location The location of the module in reference to the center of the robot.
+ * NOTE: In relation to the robot [+X is forward, +Y is left, and +THETA is Counter Clock-Wise].
+ */
+open class SwerveModule(
+ private val name: String,
+ private val drivingMotor: WrappedMotor,
+ private val turningMotor: WrappedMotor,
+ val driveController: PIDController,
+ val turnController: PIDController,
+ private val driveFeedforward: SimpleMotorFeedforward,
+ val location: Translation2d
+) {
+ init {
+ turnController.enableContinuousInput(.0, 2 * PI)
+ driveController.reset()
+ turnController.reset()
+ }
+
+ val desiredState = SwerveModuleState(
+ 0.0,
+ Rotation2d()
+ )
+
+ /** The module's [SwerveModuleState], containing speed and angle. */
+ open var state: SwerveModuleState
+ get() {
+ return SwerveModuleState(
+ drivingMotor.velocity,
+ Rotation2d(turningMotor.position)
+ )
+ }
+ set(desState) {
+ if (abs(desState.speedMetersPerSecond) < .001) {
+ stop()
+ return
+ }
+ /** Ensure the module doesn't turn more than 90 degrees. */
+ val optimizedState = SwerveModuleState.optimize(
+ desState,
+ Rotation2d(turningMotor.position)
+ )
+
+ turnController.setpoint = optimizedState.angle.radians
+ driveController.setpoint = optimizedState.speedMetersPerSecond
+ desiredState.speedMetersPerSecond = optimizedState.speedMetersPerSecond
+ desiredState.angle = optimizedState.angle
+ }
+
+ /** The module's [SwerveModulePosition], containing distance and angle. */
+ open val position: SwerveModulePosition
+ get() {
+ return SwerveModulePosition(
+ drivingMotor.position,
+ Rotation2d(turningMotor.position)
+ )
+ }
+
+ fun setVoltage(volts: Double) {
+ driveController.setpoint = 0.0
+ desiredState.speedMetersPerSecond = 0.0
+ turnController.setpoint = 0.0
+
+ turningMotor.setVoltage(turnController.calculate(turningMotor.position))
+ drivingMotor.setVoltage(volts)
+ }
+
+ fun lastDrivingVoltage(): Double {
+ return drivingMotor.lastVoltage
+ }
+
+ fun lastSteeringVoltage(): Double {
+ return turningMotor.lastVoltage
+ }
+
+ /** Set module speed to zero but keep module angle the same. */
+ fun stop() {
+ turnController.setpoint = turningMotor.position
+ desiredState.speedMetersPerSecond = 0.0
+ }
+
+ open fun update() {
+ /** CONTROL speed of module */
+ val drivePid = driveController.calculate(
+ drivingMotor.velocity
+ )
+ val driveFF = driveFeedforward.calculate(
+ desiredState.speedMetersPerSecond
+ )
+ drivingMotor.setVoltage(drivePid + driveFF)
+
+ /** CONTROL direction of module */
+ val turnPid = turnController.calculate(
+ turningMotor.position
+ )
+
+ turningMotor.set(
+ turnPid +
+ sign(desiredState.angle.radians - turningMotor.position) *
+ SwerveConstants.STEER_KS
+ )
+ }
+
+ companion object {
+ /** Create a real or simulated [SwerveModule] based on the simulation status of the robot. */
+ fun create(
+ name: String,
+ drivingMotor: WrappedMotor,
+ turningMotor: WrappedMotor,
+ driveController: PIDController,
+ turnController: PIDController,
+ driveFeedforward: SimpleMotorFeedforward,
+ location: Translation2d
+ ): SwerveModule {
+ if (RobotBase.isReal()) {
+ return SwerveModule(
+ name,
+ drivingMotor,
+ turningMotor,
+ driveController,
+ turnController,
+ driveFeedforward,
+ location
+ )
+ } else {
+ return SwerveModuleSim(
+ name,
+ drivingMotor,
+ turningMotor,
+ driveController,
+ turnController,
+ driveFeedforward,
+ location
+ )
+ }
+ }
+ }
+}
+
+/** A "simulated" swerve module. Immediately reaches to its desired state. */
+class SwerveModuleSim(
+ name: String,
+ drivingMotor: WrappedMotor,
+ turningMotor: WrappedMotor,
+ driveController: PIDController,
+ turnController: PIDController,
+ driveFeedforward: SimpleMotorFeedforward,
+ location: Translation2d
+) : SwerveModule(
+ name,
+ drivingMotor,
+ turningMotor,
+ driveController,
+ turnController,
+ driveFeedforward,
+ location
+) {
+ private val turningMotorEncoder = Encoder.SimController(turningMotor.encoder)
+ private val driveEncoder = Encoder.SimController(drivingMotor.encoder)
+ private var prevTime = Timer.getFPGATimestamp()
+ override var state: SwerveModuleState
+ get() = SwerveModuleState(
+ driveEncoder.velocity,
+ Rotation2d(turningMotorEncoder.position)
+ )
+ set(desiredState) {
+ super.state = desiredState
+ turningMotorEncoder.position = desiredState.angle.radians
+ driveEncoder.velocity = desiredState.speedMetersPerSecond
+ }
+ override val position: SwerveModulePosition
+ get() = SwerveModulePosition(
+ driveEncoder.position,
+ Rotation2d(turningMotorEncoder.position)
+ )
+
+ override fun update() {
+ val currTime = Timer.getFPGATimestamp()
+ driveEncoder.position += driveEncoder.velocity * (currTime - prevTime)
+ prevTime = currTime
+ }
+}
diff --git a/src/main/kotlin/frc/team449/control/holonomic/SwerveOrthogonalCommand.kt b/src/main/kotlin/frc/team449/control/holonomic/SwerveOrthogonalCommand.kt
new file mode 100644
index 0000000..dee846d
--- /dev/null
+++ b/src/main/kotlin/frc/team449/control/holonomic/SwerveOrthogonalCommand.kt
@@ -0,0 +1,189 @@
+package frc.team449.control.holonomic
+
+import edu.wpi.first.math.MathUtil
+import edu.wpi.first.math.filter.SlewRateLimiter
+import edu.wpi.first.math.geometry.Rotation2d
+import edu.wpi.first.math.geometry.Translation2d
+import edu.wpi.first.math.kinematics.ChassisSpeeds
+import edu.wpi.first.util.sendable.SendableBuilder
+import edu.wpi.first.wpilibj.DriverStation
+import edu.wpi.first.wpilibj.Timer
+import edu.wpi.first.wpilibj.XboxController
+import edu.wpi.first.wpilibj2.command.Command
+import frc.team449.robot2023.constants.RobotConstants
+import frc.team449.robot2023.constants.drives.SwerveConstants
+import kotlin.jvm.optionals.getOrNull
+import kotlin.math.*
+
+class SwerveOrthogonalCommand(
+ private val drive: SwerveDrive,
+ private val controller: XboxController,
+ private val fieldOriented: () -> Boolean = { true }
+) : Command() {
+
+ private var prevX = 0.0
+ private var prevY = 0.0
+
+ private var prevTime = 0.0
+
+ private var dx = 0.0
+ private var dy = 0.0
+ private var magAcc = 0.0
+ private var dt = 0.0
+ private var magAccClamped = 0.0
+
+ private var rotScaled = 0.0
+ private val allianceCompensation = { if (DriverStation.getAlliance().getOrNull() == DriverStation.Alliance.Red) 0.0 else PI }
+ private val directionCompensation = { if (DriverStation.getAlliance().getOrNull() == DriverStation.Alliance.Red) -1.0 else 1.0 }
+
+ private var atGoal = true
+
+ private var rotRamp = SlewRateLimiter(RobotConstants.ROT_RATE_LIMIT)
+
+ private val timer = Timer()
+
+ private val rotCtrl = RobotConstants.ORTHOGONAL_CONTROLLER
+
+ private var skewConstant = 11.5
+
+ private var desiredVel = doubleArrayOf(0.0, 0.0, 0.0)
+
+ init {
+ addRequirements(drive)
+ rotCtrl.enableContinuousInput(-PI, PI)
+ rotCtrl.setTolerance(0.025)
+ }
+
+ override fun initialize() {
+ timer.restart()
+
+ prevX = drive.currentSpeeds.vxMetersPerSecond
+ prevY = drive.currentSpeeds.vyMetersPerSecond
+ prevTime = 0.0
+ dx = 0.0
+ dy = 0.0
+ magAcc = 0.0
+ dt = 0.0
+ magAccClamped = 0.0
+
+ rotRamp = SlewRateLimiter(
+ RobotConstants.ROT_RATE_LIMIT,
+ RobotConstants.NEG_ROT_RATE_LIM,
+ drive.currentSpeeds.omegaRadiansPerSecond
+ )
+
+ var atGoal = true
+ }
+
+ override fun execute() {
+ val currTime = timer.get()
+ dt = currTime - prevTime
+ prevTime = currTime
+
+ val ctrlX = if (abs(controller.leftY) < RobotConstants.TRANSLATION_DEADBAND) .0 else -controller.leftY
+ val ctrlY = if (abs(controller.leftX) < RobotConstants.TRANSLATION_DEADBAND) .0 else -controller.leftX
+
+ val ctrlRadius = sqrt(ctrlX.pow(2) + ctrlY.pow(2)).pow(SwerveConstants.JOYSTICK_FILTER_ORDER)
+
+ val ctrlTheta = atan2(ctrlY, ctrlX)
+
+ val xScaled = ctrlRadius * cos(ctrlTheta) * RobotConstants.MAX_LINEAR_SPEED
+ val yScaled = ctrlRadius * sin(ctrlTheta) * RobotConstants.MAX_LINEAR_SPEED
+
+ dx = xScaled - prevX
+ dy = yScaled - prevY
+ magAcc = hypot(dx / dt, dy / dt)
+ magAccClamped = MathUtil.clamp(magAcc, -RobotConstants.MAX_ACCEL, RobotConstants.MAX_ACCEL)
+
+ val factor = if (magAcc == 0.0) 0.0 else magAccClamped / magAcc
+ val dxClamped = dx * factor
+ val dyClamped = dy * factor
+ val xClamped = prevX + dxClamped
+ val yClamped = prevY + dyClamped
+
+ prevX = xClamped
+ prevY = yClamped
+
+ if (controller.xButtonPressed) {
+ val desAngleA = MathUtil.angleModulus(7 * PI / 4 + allianceCompensation.invoke())
+ if (abs(desAngleA - drive.heading.radians) > 0.075 && abs(desAngleA - drive.heading.radians) < 2 * PI - 0.075) {
+ atGoal = false
+ rotCtrl.setpoint = desAngleA
+ }
+ } else if (controller.yButtonPressed) {
+ val desAngleY = MathUtil.angleModulus(3 * PI / 2 + allianceCompensation.invoke())
+ if (abs(desAngleY - drive.heading.radians) > 0.075 && abs(desAngleY - drive.heading.radians) < 2 * PI - 0.075) {
+ atGoal = false
+ rotCtrl.setpoint = desAngleY
+ }
+ }
+
+ if (atGoal) {
+ rotScaled = rotRamp.calculate(
+ (if (abs(controller.rightX) < RobotConstants.ROTATION_DEADBAND) .0 else -controller.rightX) *
+ drive.maxRotSpeed
+ )
+ } else {
+ rotScaled = MathUtil.clamp(
+ rotCtrl.calculate(drive.heading.radians),
+ -RobotConstants.ALIGN_ROT_SPEED,
+ RobotConstants.ALIGN_ROT_SPEED
+ )
+ atGoal = rotCtrl.atSetpoint()
+ }
+
+ val vel = Translation2d(xClamped, yClamped)
+
+ if (fieldOriented.invoke()) {
+ /** Quick fix for the velocity skewing towards the direction of rotation
+ * by rotating it with offset proportional to how much we are rotating
+ **/
+ vel.rotateBy(Rotation2d(-rotScaled * dt * skewConstant))
+
+ val desVel = ChassisSpeeds.fromFieldRelativeSpeeds(
+ vel.x * directionCompensation.invoke(),
+ vel.y * directionCompensation.invoke(),
+ rotScaled,
+ drive.heading
+ )
+ drive.set(
+ desVel
+ )
+
+ desiredVel[0] = desVel.vxMetersPerSecond
+ desiredVel[1] = desVel.vyMetersPerSecond
+ desiredVel[2] = desVel.omegaRadiansPerSecond
+ } else {
+ drive.set(
+ ChassisSpeeds(
+ vel.x,
+ vel.y,
+ rotScaled
+ )
+ )
+ }
+ }
+
+ override fun initSendable(builder: SendableBuilder) {
+ builder.publishConstString("1.0", "Controller X and Y Values")
+ builder.addDoubleProperty("1.1 currX", { if (abs(controller.leftY) < RobotConstants.TRANSLATION_DEADBAND) .0 else -controller.leftY }, null)
+ builder.addDoubleProperty("1.2 currY", { if (abs(controller.leftX) < RobotConstants.TRANSLATION_DEADBAND) .0 else -controller.leftX }, null)
+ builder.addDoubleProperty("1.3 prevX", { prevX }, null)
+ builder.addDoubleProperty("1.4 prevY", { prevY }, null)
+
+ builder.publishConstString("2.0", "Delta X, Y, Time over one loop")
+ builder.addDoubleProperty("2.1 dx", { dx }, null)
+ builder.addDoubleProperty("2.2 dy", { dy }, null)
+ builder.addDoubleProperty("2.3 dt", { dt }, null)
+
+ builder.publishConstString("3.0", "Magnitude of Acceleration")
+ builder.addDoubleProperty("3.1 magAcc", { magAcc }, null)
+ builder.addDoubleProperty("3.2 magAccClamped", { magAccClamped }, null)
+
+ builder.publishConstString("4.0", "Turning Skew")
+ builder.addDoubleProperty("4.1 skew constant", { skewConstant }, { k: Double -> skewConstant = k })
+
+ builder.publishConstString("5.0", "Given Speeds")
+ builder.addDoubleArrayProperty("Chassis Speed", { desiredVel }, null)
+ }
+}
diff --git a/src/main/kotlin/frc/team449/control/holonomic/SwerveSim.kt b/src/main/kotlin/frc/team449/control/holonomic/SwerveSim.kt
new file mode 100644
index 0000000..fbee4da
--- /dev/null
+++ b/src/main/kotlin/frc/team449/control/holonomic/SwerveSim.kt
@@ -0,0 +1,139 @@
+package frc.team449.control.holonomic
+
+import edu.wpi.first.math.geometry.Pose2d
+import edu.wpi.first.math.geometry.Rotation2d
+import edu.wpi.first.math.kinematics.SwerveDriveOdometry
+import edu.wpi.first.wpilibj.DriverStation
+import edu.wpi.first.wpilibj.Timer.getFPGATimestamp
+import edu.wpi.first.wpilibj.smartdashboard.Field2d
+import frc.team449.control.vision.VisionSubsystem
+import frc.team449.robot2023.constants.vision.VisionConstants
+import frc.team449.system.AHRS
+import kotlin.math.abs
+import kotlin.math.hypot
+import kotlin.math.pow
+import kotlin.math.sqrt
+
+class SwerveSim(
+ modules: List,
+ ahrs: AHRS,
+ maxLinearSpeed: Double,
+ maxRotSpeed: Double,
+ cameras: List,
+ field: Field2d
+) : SwerveDrive(modules, ahrs, maxLinearSpeed, maxRotSpeed, cameras, field) {
+
+ private var lastTime = getFPGATimestamp()
+ var odoPose = Pose2d()
+ var currHeading = Rotation2d()
+
+ private val odometry = SwerveDriveOdometry(
+ kinematics,
+ currHeading,
+ getPositions()
+ )
+
+ /** The (x, y, theta) position of the robot on the field. */
+ override var pose: Pose2d
+ get() = this.poseEstimator.estimatedPosition
+ set(value) {
+ this.poseEstimator.resetPosition(
+ currHeading,
+ getPositions(),
+ value
+ )
+
+ odometry.resetPosition(
+ currHeading,
+ getPositions(),
+ value
+ )
+ }
+
+ override fun localize() = try {
+ for ((index, camera) in cameras.withIndex()) {
+ val result = camera.estimatedPose(Pose2d(pose.x, pose.y, currHeading))
+ if (result.isPresent) {
+ val presentResult = result.get()
+ numTargets[index] = presentResult.targetsUsed.size.toDouble()
+ tagDistance[index] = 0.0
+ avgAmbiguity[index] = 0.0
+ heightError[index] = abs(presentResult.estimatedPose.z)
+
+ for (tag in presentResult.targetsUsed) {
+ val tagPose = camera.estimator.fieldTags.getTagPose(tag.fiducialId)
+ if (tagPose.isPresent) {
+ val estimatedToTag = presentResult.estimatedPose.minus(tagPose.get())
+ tagDistance[index] += sqrt(estimatedToTag.x.pow(2) + estimatedToTag.y.pow(2)) / numTargets[index]
+ avgAmbiguity[index] = tag.poseAmbiguity / numTargets[index]
+ } else {
+ tagDistance[index] = Double.MAX_VALUE
+ avgAmbiguity[index] = Double.MAX_VALUE
+ break
+ }
+ }
+
+ visionPose = presentResult.estimatedPose.toPose2d()
+
+ if (presentResult.timestampSeconds > 0 &&
+ avgAmbiguity[index] <= VisionConstants.MAX_AMBIGUITY &&
+ numTargets[index] < 2 && tagDistance[index] <= VisionConstants.MAX_DISTANCE_SINGLE_TAG ||
+ numTargets[index] >= 2 && tagDistance[index] <= VisionConstants.MAX_DISTANCE_MULTI_TAG * (1 + (numTargets[index] - 2) * VisionConstants.TAG_MULTIPLIER) &&
+ heightError[index] < VisionConstants.MAX_HEIGHT_ERR_METERS
+ ) {
+ poseEstimator.addVisionMeasurement(
+ visionPose,
+ presentResult.timestampSeconds,
+ camera.getEstimationStdDevs(numTargets[index].toInt(), tagDistance[index])
+ )
+ usedVision[index] = true
+ } else {
+ usedVision[index] = false
+ }
+ }
+ }
+ } catch (e: Error) {
+ DriverStation.reportError(
+ "!!!!!!!!! VISION ERROR !!!!!!!",
+ e.stackTrace
+ )
+ }
+
+ override fun periodic() {
+ val currTime = getFPGATimestamp()
+
+ currHeading = currHeading.plus(Rotation2d(super.desiredSpeeds.omegaRadiansPerSecond * (currTime - lastTime)))
+ this.lastTime = currTime
+
+ set(super.desiredSpeeds)
+
+ // Updates the robot's currentSpeeds.
+ currentSpeeds = kinematics.toChassisSpeeds(
+ modules[0].state,
+ modules[1].state,
+ modules[2].state,
+ modules[3].state
+ )
+
+ val transVel = hypot(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond)
+ if (transVel > maxSpeed) maxSpeed = transVel
+
+ // Update the robot's pose using the gyro heading and the SwerveModulePositions of each module.
+ this.poseEstimator.update(
+ currHeading,
+ getPositions()
+ )
+
+ if (cameras.isNotEmpty()) localize()
+
+ // Sets the robot's pose and individual module rotations on the SmartDashboard [Field2d] widget.
+ setRobotPose()
+
+ odoPose = odometry.update(
+ currHeading,
+ getPositions()
+ )
+
+ field.getObject("odo").pose = odoPose
+ }
+}
diff --git a/src/main/kotlin/frc/team449/control/vision/VisionEstimator.kt b/src/main/kotlin/frc/team449/control/vision/VisionEstimator.kt
new file mode 100644
index 0000000..8598165
--- /dev/null
+++ b/src/main/kotlin/frc/team449/control/vision/VisionEstimator.kt
@@ -0,0 +1,303 @@
+package frc.team449.control.vision
+
+import edu.wpi.first.apriltag.AprilTag
+import edu.wpi.first.apriltag.AprilTagFieldLayout
+import edu.wpi.first.math.Matrix
+import edu.wpi.first.math.geometry.*
+import edu.wpi.first.math.numbers.N1
+import edu.wpi.first.math.numbers.N3
+import edu.wpi.first.math.numbers.N5
+import edu.wpi.first.wpilibj.DriverStation
+import frc.team449.robot2023.constants.vision.VisionConstants
+import org.photonvision.EstimatedRobotPose
+import org.photonvision.PhotonCamera
+import org.photonvision.PhotonPoseEstimator
+import org.photonvision.estimation.OpenCVHelp
+import org.photonvision.targeting.PNPResults
+import org.photonvision.targeting.PhotonPipelineResult
+import org.photonvision.targeting.PhotonTrackedTarget
+import org.photonvision.targeting.TargetCorner
+import java.util.Optional
+import kotlin.math.PI
+import kotlin.math.abs
+
+/**
+ * This class uses normal multi-tag PNP and lowest ambiguity using the gyro rotation
+ * for the internal cam-to-tag transform as a fallback strategy
+ */
+class VisionEstimator(
+ private val tagLayout: AprilTagFieldLayout,
+ val camera: PhotonCamera,
+ private val robotToCam: Transform3d
+) : PhotonPoseEstimator(tagLayout, PoseStrategy.MULTI_TAG_PNP_ON_RIO, camera, robotToCam) {
+ private val reportedErrors: HashSet = HashSet()
+ private var driveHeading: Rotation2d? = null
+ private var lastPose: Pose3d? = null
+
+ fun estimatedPose(currPose: Pose2d): Optional {
+ driveHeading = currPose.rotation
+ lastPose = Pose3d(currPose.x, currPose.y, 0.0, Rotation3d(0.0, 0.0, currPose.rotation.radians))
+ return updatePose(camera.latestResult)
+ }
+
+ private fun updatePose(cameraResult: PhotonPipelineResult?): Optional {
+ // Time in the past -- give up, since the following if expects times > 0
+ if (cameraResult!!.timestampSeconds < 0) {
+ return Optional.empty()
+ }
+
+ // If the pose cache timestamp was set, and the result is from the same timestamp, return an
+ // empty result
+ if (poseCacheTimestampSeconds > 0 &&
+ abs(poseCacheTimestampSeconds - cameraResult.timestampSeconds) < 1e-6
+ ) {
+ return Optional.empty()
+ }
+
+ // Remember the timestamp of the current result used
+ poseCacheTimestampSeconds = cameraResult.timestampSeconds
+
+ // If no targets seen, trivial case -- return empty result
+ return if (!cameraResult.hasTargets()) {
+ Optional.empty()
+ } else {
+ multiTagPNPStrategy(cameraResult)
+ }
+ }
+
+ private fun checkBest(check: Pose3d?, opt1: Pose3d?, opt2: Pose3d?): Pose3d? {
+ if (check == null || opt1 == null || opt2 == null) return null
+ val dist1 = check.translation.toTranslation2d().getDistance(opt1.translation.toTranslation2d())
+ val dist2 = check.translation.toTranslation2d().getDistance(opt2.translation.toTranslation2d())
+
+ return if (dist1 < dist2) {
+ opt1
+ } else {
+ opt2
+ }
+ }
+
+ private fun multiTagPNPStrategy(result: PhotonPipelineResult): Optional {
+ // Arrays we need declared up front
+ val visCorners = ArrayList()
+ val knownVisTags = ArrayList()
+ val fieldToCams = ArrayList()
+ val fieldToCamsAlt = ArrayList()
+ val usedTargets = ArrayList()
+ if (result.getTargets().size < 2) {
+ // Run fallback strategy instead
+ return lowestAmbiguityStrategy(result)
+ }
+ for (target: PhotonTrackedTarget in result.getTargets()) {
+ val tagPoseOpt = tagLayout.getTagPose(target.fiducialId)
+ if (tagPoseOpt.isEmpty) {
+ reportFiducialPoseError(target.fiducialId)
+ continue
+ }
+ val tagPose = tagPoseOpt.get()
+
+ usedTargets.add(target)
+
+ visCorners.addAll(target.detectedCorners)
+ // actual layout poses of visible tags -- not exposed, so have to recreate
+ knownVisTags.add(AprilTag(target.fiducialId, tagPose))
+ fieldToCams.add(tagPose.transformBy(target.bestCameraToTarget.inverse()))
+ fieldToCamsAlt.add(tagPose.transformBy(target.alternateCameraToTarget.inverse()))
+ }
+ val cameraMatrixOpt = camera.cameraMatrix
+ val distCoeffsOpt = camera.distCoeffs
+ val hasCalibData = cameraMatrixOpt.isPresent && distCoeffsOpt.isPresent
+
+ // multi-target solvePNP
+ if (hasCalibData && visCorners.size == knownVisTags.size * 4 && knownVisTags.isNotEmpty()) {
+ val cameraMatrix = cameraMatrixOpt.get()
+ val distCoeffs = distCoeffsOpt.get()
+ val pnpResults = estimateCamPosePNP(cameraMatrix, distCoeffs, usedTargets.toList())
+ val best = Pose3d()
+ .plus(pnpResults.best) // field-to-camera
+ .plus(robotToCam.inverse()) // field-to-robot
+
+ val alternate = Pose3d()
+ .plus(pnpResults.alt)
+ .plus(robotToCam.inverse())
+
+ return Optional.of(
+ EstimatedRobotPose(
+ checkBest(lastPose, best, alternate) ?: best,
+ result.timestampSeconds,
+ result.targets,
+ PoseStrategy.MULTI_TAG_PNP_ON_RIO
+ )
+ )
+ }
+
+ return Optional.empty()
+ }
+
+ /**
+ * Return the estimated position of the robot with the lowest position ambiguity from a List of
+ * pipeline results.
+ *
+ * @param result pipeline result
+ * @return the estimated position of the robot in the FCS and the estimated timestamp of this
+ * estimation.
+ */
+ private fun lowestAmbiguityStrategy(result: PhotonPipelineResult): Optional {
+ var lowestAmbiguityTarget: PhotonTrackedTarget? = null
+ var lowestAmbiguityScore = 10.0
+ for (target: PhotonTrackedTarget in result.targets) {
+ val targetPoseAmbiguity = target.poseAmbiguity
+ // Make sure the target is a Fiducial target.
+ if (targetPoseAmbiguity != -1.0 && targetPoseAmbiguity < lowestAmbiguityScore) {
+ lowestAmbiguityScore = targetPoseAmbiguity
+ lowestAmbiguityTarget = target
+ }
+ }
+
+ // Although there are confirmed to be targets, none of them may be fiducial
+ // targets.
+ if (lowestAmbiguityTarget == null) return Optional.empty()
+ val targetFiducialId = lowestAmbiguityTarget.fiducialId
+ val targetPosition = tagLayout.getTagPose(targetFiducialId)
+ if (targetPosition.isEmpty) {
+ reportFiducialPoseError(targetFiducialId)
+ return Optional.empty()
+ }
+
+ val bestPose = targetPosition
+ .get()
+ .transformBy(
+ Transform3d(
+ lowestAmbiguityTarget.bestCameraToTarget.translation,
+ Rotation3d(
+ lowestAmbiguityTarget.bestCameraToTarget.rotation.x,
+ lowestAmbiguityTarget.bestCameraToTarget.rotation.y,
+ driveHeading!!.radians + robotToCam.rotation.z - targetPosition.get().rotation.z
+ )
+ ).inverse()
+ )
+ .transformBy(robotToCam.inverse())
+
+ val altPose = targetPosition
+ .get()
+ .transformBy(
+ Transform3d(
+ lowestAmbiguityTarget.alternateCameraToTarget.translation,
+ Rotation3d(
+ lowestAmbiguityTarget.alternateCameraToTarget.rotation.x,
+ lowestAmbiguityTarget.alternateCameraToTarget.rotation.y,
+ driveHeading!!.radians + robotToCam.rotation.z - targetPosition.get().rotation.z
+ )
+ ).inverse()
+ )
+ .transformBy(robotToCam.inverse())
+
+ val usedPose = checkBest(lastPose, bestPose, altPose) ?: bestPose
+
+ if (usedPose == bestPose) {
+ val camBestPose = targetPosition
+ .get()
+ .transformBy(
+ Transform3d(
+ lowestAmbiguityTarget.bestCameraToTarget.translation,
+ lowestAmbiguityTarget.bestCameraToTarget.rotation
+ ).inverse()
+ )
+ .transformBy(robotToCam.inverse())
+
+ if (abs(camBestPose.rotation.z * (180 / (2 * PI)) - driveHeading!!.degrees) > VisionConstants.SINGLE_TAG_HEADING_MAX_DEV_DEG) {
+ DriverStation.reportWarning("Best Single Tag Heading over Max Deviation, deviated by ${abs(camBestPose.rotation.z * (180 / (2 * PI)) - driveHeading!!.degrees)}", false)
+ return Optional.empty()
+ }
+ } else {
+
+ val camAltPose = targetPosition
+ .get()
+ .transformBy(
+ Transform3d(
+ lowestAmbiguityTarget.alternateCameraToTarget.translation,
+ lowestAmbiguityTarget.alternateCameraToTarget.rotation
+ ).inverse()
+ )
+ .transformBy(robotToCam.inverse())
+
+ if (abs(camAltPose.rotation.z * (180 / (2 * PI)) - driveHeading!!.degrees) > VisionConstants.SINGLE_TAG_HEADING_MAX_DEV_DEG) {
+ DriverStation.reportWarning("Alt Single Tag Heading over Max Deviation, deviated by ${abs(camAltPose.rotation.z * (180 / (2 * PI)) - driveHeading!!.degrees)}", false)
+ return Optional.empty()
+ }
+ }
+
+ return Optional.of(
+ EstimatedRobotPose(
+ checkBest(lastPose, bestPose, altPose) ?: bestPose,
+ result.timestampSeconds,
+ mutableListOf(lowestAmbiguityTarget),
+ PoseStrategy.LOWEST_AMBIGUITY
+ )
+ )
+ }
+
+ private fun reportFiducialPoseError(fiducialId: Int) {
+ if (!reportedErrors.contains(fiducialId)) {
+ DriverStation.reportError(
+ "[PhotonPoseEstimator] Tried to get pose of unknown AprilTag: $fiducialId",
+ false
+ )
+ reportedErrors.add(fiducialId)
+ }
+ }
+
+ private fun estimateCamPosePNP(
+ cameraMatrix: Matrix?,
+ distCoeffs: Matrix?,
+ visTags: List?,
+ ): PNPResults {
+ if (visTags == null || tagLayout.tags.isEmpty() || visTags.isEmpty()) {
+ return PNPResults()
+ }
+ val corners = java.util.ArrayList()
+ val knownTags = java.util.ArrayList()
+ // ensure these are AprilTags in our layout
+ for (tgt in visTags) {
+ val id = tgt.fiducialId
+ tagLayout
+ .getTagPose(id)
+ .ifPresent { pose: Pose3d? ->
+ knownTags.add(AprilTag(id, pose))
+ corners.addAll(tgt.detectedCorners)
+ }
+ }
+ if (knownTags.size == 0 || corners.size == 0 || corners.size % 4 != 0) {
+ return PNPResults()
+ }
+ val points = OpenCVHelp.cornersToPoints(corners)
+
+ // single-tag pnp
+ return if (knownTags.size == 1) {
+ val camToTag = OpenCVHelp.solvePNP_SQUARE(cameraMatrix, distCoeffs, VisionConstants.TAG_MODEL.vertices, points)
+ if (!camToTag.isPresent) return PNPResults()
+ val bestPose = knownTags[0].pose.transformBy(camToTag.best.inverse())
+ var altPose: Pose3d? = Pose3d()
+ if (camToTag.ambiguity != 0.0) altPose = knownTags[0].pose.transformBy(camToTag.alt.inverse())
+ val o = Pose3d()
+ PNPResults(
+ Transform3d(o, bestPose),
+ Transform3d(o, altPose),
+ camToTag.ambiguity,
+ camToTag.bestReprojErr,
+ camToTag.altReprojErr
+ )
+ } else {
+ val objectTrls = java.util.ArrayList()
+ for (tag in knownTags) objectTrls.addAll(VisionConstants.TAG_MODEL.getFieldVertices(tag.pose))
+ val camToOrigin = OpenCVHelp.solvePNP_SQPNP(cameraMatrix, distCoeffs, objectTrls, points)
+ if (!camToOrigin.isPresent) PNPResults() else PNPResults(
+ camToOrigin.best.inverse(),
+ camToOrigin.alt.inverse(),
+ camToOrigin.ambiguity,
+ camToOrigin.bestReprojErr,
+ camToOrigin.altReprojErr
+ )
+ }
+ }
+}
diff --git a/src/main/kotlin/frc/team449/control/vision/VisionSubsystem.kt b/src/main/kotlin/frc/team449/control/vision/VisionSubsystem.kt
new file mode 100644
index 0000000..1681e2d
--- /dev/null
+++ b/src/main/kotlin/frc/team449/control/vision/VisionSubsystem.kt
@@ -0,0 +1,106 @@
+package frc.team449.control.vision
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout
+import edu.wpi.first.math.Matrix
+import edu.wpi.first.math.geometry.Pose2d
+import edu.wpi.first.math.geometry.Rotation2d
+import edu.wpi.first.math.geometry.Transform3d
+import edu.wpi.first.math.numbers.N1
+import edu.wpi.first.math.numbers.N3
+import edu.wpi.first.wpilibj.RobotBase
+import edu.wpi.first.wpilibj.smartdashboard.Field2d
+import frc.team449.robot2023.constants.vision.VisionConstants
+import org.photonvision.EstimatedRobotPose
+import org.photonvision.PhotonCamera
+import org.photonvision.simulation.PhotonCameraSim
+import org.photonvision.simulation.SimCameraProperties
+import org.photonvision.simulation.VisionSystemSim
+import java.util.Optional
+import kotlin.math.abs
+import kotlin.math.pow
+
+
+class VisionSubsystem(
+ name: String,
+ tagLayout: AprilTagFieldLayout,
+ val robotToCam: Transform3d,
+ private val visionSystemSim: VisionSystemSim?
+) {
+ val estimator = VisionEstimator(
+ tagLayout,
+ PhotonCamera(name),
+ robotToCam
+ )
+
+ private var lastEstTimestamp = 0.0
+
+ private var cameraSim: PhotonCameraSim? = null
+
+ init {
+ if (RobotBase.isSimulation()) {
+ visionSystemSim!!.addAprilTags(tagLayout)
+
+ val cameraProp = SimCameraProperties()
+ cameraProp.setCalibration(VisionConstants.SIM_CAMERA_WIDTH_PX, VisionConstants.SIM_CAMERA_HEIGHT_PX, Rotation2d.fromDegrees(VisionConstants.SIM_FOV_DEG))
+ cameraProp.setCalibError(VisionConstants.SIM_CALIB_AVG_ERR_PX, VisionConstants.SIM_CALIB_ERR_STDDEV_PX)
+ cameraProp.fps = VisionConstants.SIM_FPS
+ cameraProp.avgLatencyMs = VisionConstants.SIM_AVG_LATENCY
+ cameraProp.latencyStdDevMs = VisionConstants.SIM_STDDEV_LATENCY
+
+ cameraSim = PhotonCameraSim(estimator.camera, cameraProp)
+
+ cameraSim!!.enableDrawWireframe(VisionConstants.ENABLE_WIREFRAME)
+
+ visionSystemSim.addCamera(cameraSim, robotToCam)
+ }
+ }
+
+ private fun getSimDebugField(): Field2d? {
+ return if (!RobotBase.isSimulation()) null else visionSystemSim!!.debugField
+ }
+
+ fun estimatedPose(currPose: Pose2d): Optional {
+ val visionEst = estimator.estimatedPose(currPose)
+ val latestTimestamp = estimator.camera.latestResult.timestampSeconds
+ val newResult = abs(latestTimestamp - lastEstTimestamp) > 1e-4
+ if (RobotBase.isSimulation()) {
+ visionEst.ifPresentOrElse(
+ { est ->
+ getSimDebugField()!!
+ .getObject("VisionEstimation").pose = est.estimatedPose.toPose2d()
+ }
+ ) { if (newResult) getSimDebugField()!!.getObject("VisionEstimation").setPoses() }
+ }
+ return if (newResult) {
+ lastEstTimestamp = latestTimestamp
+ visionEst
+ } else {
+ Optional.empty()
+ }
+ }
+
+ fun getEstimationStdDevs(numTags: Int, avgDist: Double): Matrix {
+ var estStdDevs = VisionConstants.SINGLE_TAG_TRUST.copy()
+
+ if (numTags == 0) return estStdDevs
+
+ // Decrease std devs if multiple targets are visible
+ if (numTags > 1) estStdDevs = VisionConstants.MULTI_TAG_TRUST.copy()
+
+ // Increase std devs based on (average) distance
+ estStdDevs.times(
+ 1 + avgDist.pow(VisionConstants.ORDER) * VisionConstants.PROPORTION
+ )
+
+ return estStdDevs
+ }
+
+ fun simulationPeriodic(robotSimPose: Pose2d?) {
+ visionSystemSim!!.update(robotSimPose)
+ }
+
+ /** Reset pose history of the robot in the vision system simulation. */
+ fun resetSimPose(pose: Pose2d?) {
+ if (RobotBase.isSimulation()) visionSystemSim!!.resetRobotPose(pose)
+ }
+}
\ No newline at end of file
diff --git a/src/main/kotlin/frc/team449/robot2023/Robot.kt b/src/main/kotlin/frc/team449/robot2023/Robot.kt
new file mode 100644
index 0000000..c53308e
--- /dev/null
+++ b/src/main/kotlin/frc/team449/robot2023/Robot.kt
@@ -0,0 +1,40 @@
+package frc.team449.robot2023
+
+import edu.wpi.first.wpilibj.PowerDistribution
+import edu.wpi.first.wpilibj.SPI
+import edu.wpi.first.wpilibj.XboxController
+import frc.team449.RobotBase
+import frc.team449.control.holonomic.SwerveDrive
+import frc.team449.control.holonomic.SwerveOrthogonalCommand
+import frc.team449.robot2023.constants.RobotConstants
+import frc.team449.robot2023.subsystems.light.Light
+import frc.team449.system.AHRS
+import monologue.Logged
+import monologue.Monologue.LogBoth
+
+class Robot : RobotBase(), Logged {
+
+ val driveController = XboxController(0)
+
+ val mechController = XboxController(1)
+
+ val ahrs = AHRS(SPI.Port.kMXP)
+
+ // Instantiate/declare PDP and other stuff here
+
+ @LogBoth
+ override val powerDistribution: PowerDistribution = PowerDistribution(
+ RobotConstants.PDH_CAN,
+ PowerDistribution.ModuleType.kRev
+ )
+
+ @LogBoth
+ override val drive = SwerveDrive.createSwerve(ahrs, field)
+
+ @LogBoth
+ override val driveCommand = SwerveOrthogonalCommand(drive, driveController)
+
+ val light = Light.createLight()
+//
+// val infrared = DigitalInput(RobotConstants.IR_CHANNEL)
+}
diff --git a/src/main/kotlin/frc/team449/robot2023/auto/AutoUtil.kt b/src/main/kotlin/frc/team449/robot2023/auto/AutoUtil.kt
new file mode 100644
index 0000000..a32c17c
--- /dev/null
+++ b/src/main/kotlin/frc/team449/robot2023/auto/AutoUtil.kt
@@ -0,0 +1,58 @@
+package frc.team449.robot2023.auto
+
+import edu.wpi.first.math.MatBuilder
+import edu.wpi.first.math.MathUtil
+import edu.wpi.first.math.numbers.N2
+import edu.wpi.first.math.numbers.N3
+import frc.team449.control.auto.ChoreoTrajectory
+import frc.team449.robot2023.constants.field.FieldConstants
+import kotlin.math.PI
+
+object AutoUtil {
+ fun transformForPos2(pathGroup: MutableList): MutableList {
+ for (index in 0 until pathGroup.size) {
+ for (time in pathGroup[index].objectiveTimestamps) {
+ val currentMatrix = pathGroup[index].stateMap.get(time)
+
+ val newMatrix = MatBuilder(N2.instance, N3.instance).fill(
+ currentMatrix[0, 0],
+ FieldConstants.fieldWidth - currentMatrix[0, 1],
+ -currentMatrix[0, 2],
+ currentMatrix[1, 0],
+ -currentMatrix[1, 1],
+ -currentMatrix[1, 2]
+ )
+
+ pathGroup[index].stateMap.put(time, newMatrix)
+ }
+ }
+
+ return pathGroup
+ }
+
+ fun transformForRed(pathGroup: MutableList): MutableList {
+ for (index in 0 until pathGroup.size) {
+ for (time in pathGroup[index].objectiveTimestamps) {
+ val currentMatrix = pathGroup[index].stateMap.get(time)
+
+ val newMatrix = MatBuilder(N2.instance, N3.instance).fill(
+ FieldConstants.fieldLength - currentMatrix[0, 0],
+ FieldConstants.fieldWidth - currentMatrix[0, 1],
+ MathUtil.angleModulus(PI + currentMatrix[0, 2]),
+ -currentMatrix[1, 0],
+ -currentMatrix[1, 1],
+ currentMatrix[1, 2]
+ )
+
+ pathGroup[index].stateMap.put(time, newMatrix)
+ }
+ }
+
+ return pathGroup
+ }
+
+ /** Add other methods that return commands that do groups of actions that are done
+ * across different auto routines. For Charged UP, these methods were things such as
+ * dropping a cone/cube, or getting in ground intake position, etc.
+ */
+}
diff --git a/src/main/kotlin/frc/team449/robot2023/auto/routines/DoNothing.kt b/src/main/kotlin/frc/team449/robot2023/auto/routines/DoNothing.kt
new file mode 100644
index 0000000..8832f55
--- /dev/null
+++ b/src/main/kotlin/frc/team449/robot2023/auto/routines/DoNothing.kt
@@ -0,0 +1,24 @@
+package frc.team449.robot2023.auto.routines
+
+import edu.wpi.first.wpilibj2.command.Command
+import edu.wpi.first.wpilibj2.command.InstantCommand
+import frc.team449.control.auto.ChoreoRoutine
+import frc.team449.control.auto.ChoreoRoutineStructure
+import frc.team449.control.auto.ChoreoTrajectory
+import frc.team449.robot2023.Robot
+
+class DoNothing(
+ robot: Robot
+) : ChoreoRoutineStructure {
+
+ override val routine =
+ ChoreoRoutine(
+ drive = robot.drive
+ )
+
+ override val trajectory: MutableList = mutableListOf()
+
+ override fun createCommand(): Command {
+ return InstantCommand()
+ }
+}
diff --git a/src/main/kotlin/frc/team449/robot2023/auto/routines/RoutineChooser.kt b/src/main/kotlin/frc/team449/robot2023/auto/routines/RoutineChooser.kt
new file mode 100644
index 0000000..6f0a047
--- /dev/null
+++ b/src/main/kotlin/frc/team449/robot2023/auto/routines/RoutineChooser.kt
@@ -0,0 +1,23 @@
+package frc.team449.robot2023.auto.routines
+
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser
+import edu.wpi.first.wpilibj2.command.Command
+import frc.team449.robot2023.Robot
+
+class RoutineChooser(private val robot: Robot) : SendableChooser() {
+
+ fun routineMap(): HashMap {
+ return hashMapOf(
+ "DoNothing" to DoNothing(robot).createCommand(),
+ )
+ }
+
+ init {
+ updateOptions(true)
+ }
+
+ fun updateOptions(isRed: Boolean) {
+ /** Add auto options here */
+ this.setDefaultOption("Do Nothing", "DoNothing")
+ }
+}
diff --git a/src/main/kotlin/frc/team449/robot2023/commands/characterization/Characterization.kt b/src/main/kotlin/frc/team449/robot2023/commands/characterization/Characterization.kt
new file mode 100644
index 0000000..0169ea2
--- /dev/null
+++ b/src/main/kotlin/frc/team449/robot2023/commands/characterization/Characterization.kt
@@ -0,0 +1,85 @@
+// Copyright (c) 2023 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+package frc.team449.robot2023.commands.characterization
+
+import edu.wpi.first.wpilibj.Timer
+import edu.wpi.first.wpilibj2.command.Command
+import edu.wpi.first.wpilibj2.command.Subsystem
+import java.util.LinkedList
+import java.util.function.DoubleConsumer
+import java.util.function.DoubleSupplier
+import kotlin.math.abs
+
+class Characterization(
+ subsystem: Subsystem?,
+ private val forward: Boolean,
+ private val name: String,
+ private val voltageConsumer: DoubleConsumer,
+ private val velocitySupplier: DoubleSupplier
+) : Command() {
+ private val timer = Timer()
+ private val data = FeedForwardCharacterizationData(name)
+
+ init {
+ addRequirements(subsystem)
+ }
+
+
+ // Called when the command is initially scheduled.
+ override fun initialize() {
+ timer.reset()
+ timer.start()
+ }
+
+ // Called every time the scheduler runs while the command is scheduled.
+ override fun execute() {
+ if (timer.get() < startDelaySecs) {
+ voltageConsumer.accept(0.0)
+ } else {
+ val voltage = (timer.get() - startDelaySecs) * rampRateVoltsPerSec * if (forward) 1 else -1
+ voltageConsumer.accept(voltage)
+ data.add(velocitySupplier.asDouble, voltage)
+ }
+ }
+
+ // Called once the command ends or is interrupted.
+ override fun end(interrupted: Boolean) {
+ voltageConsumer.accept(0.0)
+
+ timer.stop()
+ println("hi i ended")
+ if (!(data.velocityData.size == 0 || data.voltageData.size == 0)) {
+ val regression = Regression(
+ data.velocityData.stream().mapToDouble { obj: Double -> obj }.toArray(),
+ data.voltageData.stream().mapToDouble { obj: Double -> obj }.toArray(),
+ 1
+ )
+ println("FF Characterization Results ($name):)\n\tCount= ${data.velocityData.size}\n\tR2=${regression.R2()}\n\tkS: ${regression.beta(0)}\n\tkV: ${regression.beta(1)}")
+ }
+ }
+
+ // Returns true when the command should end.
+ override fun isFinished(): Boolean {
+ return false
+ }
+
+ class FeedForwardCharacterizationData(private val name: String) {
+ val velocityData: MutableList = LinkedList()
+ val voltageData: MutableList = LinkedList()
+ fun add(velocity: Double, voltage: Double) {
+ if (abs(velocity) > 1E-4) {
+ velocityData.add(abs(velocity))
+ voltageData.add(abs(voltage))
+ }
+ }
+ }
+
+ companion object {
+ private const val startDelaySecs = 1.5
+ private const val rampRateVoltsPerSec = 0.085
+ }
+}
\ No newline at end of file
diff --git a/src/main/kotlin/frc/team449/robot2023/commands/characterization/Regression.kt b/src/main/kotlin/frc/team449/robot2023/commands/characterization/Regression.kt
new file mode 100644
index 0000000..fec1213
--- /dev/null
+++ b/src/main/kotlin/frc/team449/robot2023/commands/characterization/Regression.kt
@@ -0,0 +1,184 @@
+// Copyright (c) 2023 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+package frc.team449.robot2023.commands.characterization
+
+import Jama.Matrix
+import Jama.QRDecomposition
+import kotlin.math.abs
+import kotlin.math.max
+import kotlin.math.pow
+
+// NOTE: This file is available at
+// http://algs4.cs.princeton.edu/14analysis/PolynomialRegression.java.html
+/**
+ * The `PolynomialRegression` class performs a polynomial regression on an set of *N*
+ * data points (*yi*, *xi*). That is, it fits a polynomial
+ * *y* = 0 + 1 *x* + 2
+ * *x*2 + ... + *d* *x**d* (where
+ * *y* is the response variable, *x* is the predictor variable, and the
+ * *i* are the regression coefficients) that minimizes the sum of squared
+ * residuals of the multiple regression model. It also computes associated the coefficient of
+ * determination *R*2.
+ *
+ *
+ * This implementation performs a QR-decomposition of the underlying Vandermonde matrix, so it is
+ * neither the fastest nor the most numerically stable way to perform the polynomial regression.
+ *
+ * @author Robert Sedgewick
+ * @author Kevin Wayne
+ */
+class Regression @JvmOverloads constructor(
+ x: DoubleArray, y: DoubleArray, // degree of the polynomial regression
+ private var degree: Int, // name of the predictor variable
+ private var variableName: String = "n"
+) : Comparable {
+ private val beta // the polynomial regression coefficients
+ : Matrix
+ private val sse // sum of squares due to error
+ : Double
+ private var sst = 0.0 // total sum of squares
+
+ init {
+ val n = x.size
+ var qr: QRDecomposition?
+ var matrixX: Matrix?
+
+ // in case Vandermonde matrix does not have full rank, reduce degree until it
+ // does
+ while (true) {
+
+ // build Vandermonde matrix
+ val vandermonde = Array(n) { DoubleArray(degree + 1) }
+ for (i in 0 until n) {
+ for (j in 0..degree) {
+ vandermonde[i][j] = x[i].pow(j.toDouble())
+ }
+ }
+ matrixX = Matrix(vandermonde)
+
+ // find least squares solution
+ qr = QRDecomposition(matrixX)
+ if (qr.isFullRank()) break
+
+ // decrease degree and try again
+ degree--
+ }
+
+ // create matrix from vector
+ val matrixY = Matrix(y, n)
+
+ // linear regression coefficients
+ beta = qr!!.solve(matrixY)
+
+ // mean of y[] values
+ var sum = 0.0
+ for (i in 0 until n) sum += y[i]
+ val mean = sum / n
+
+ // total variation to be accounted for
+ for (i in 0 until n) {
+ val dev = y[i] - mean
+ sst += dev * dev
+ }
+
+ // variation not accounted for
+ val residuals: Matrix? = matrixX!!.times(beta).minus(matrixY)
+ sse = residuals!!.norm2() * residuals.norm2()
+ }
+
+ /**
+ * Returns the `j`th regression coefficient.
+ *
+ * @param j the index
+ * @return the `j`th regression coefficient
+ */
+ fun beta(j: Int): Double {
+ // to make -0.0 print as 0.0
+ return if (abs(beta.get(j, 0)) < 1E-4) 0.0 else beta.get(j, 0)
+ }
+
+ /**
+ * Returns the degree of the polynomial to fit.
+ *
+ * @return the degree of the polynomial to fit
+ */
+ private fun degree(): Int {
+ return degree
+ }
+
+ /**
+ * Returns the coefficient of determination *R*2.
+ *
+ * @return the coefficient of determination *R*2, which is a real number between
+ * 0 and 1
+ */
+ fun R2(): Double {
+ return if (sst == 0.0) 1.0 else 1.0 - sse / sst // constant function
+ }
+
+ /**
+ * Returns the expected response `y` given the value of the predictor variable `x`.
+ *
+ * @param x the value of the predictor variable
+ * @return the expected response `y` given the value of the predictor variable `x`
+ */
+ fun predict(x: Double): Double {
+ // horner's method
+ var y = 0.0
+ for (j in degree downTo 0) y = beta(j) + x * y
+ return y
+ }
+
+ /**
+ * Returns a string representation of the polynomial regression model.
+ *
+ * @return a string representation of the polynomial regression model, including the best-fit
+ * polynomial and the coefficient of determination *R*2
+ */
+ override fun toString(): String {
+ var s = StringBuilder()
+ var j = degree
+
+ // ignoring leading zero coefficients
+ while (j >= 0 && abs(beta(j)) < 1E-5) j--
+
+ // create remaining terms
+ while (j >= 0) {
+ when (j) {
+ 0 -> s.append(String.format("%.10f ", beta(j)))
+ 1 -> s.append(String.format("%.10f %s + ", beta(j), variableName))
+ else -> s.append(
+ String.format(
+ "%.10f %s^%d + ", beta(j),
+ variableName, j
+ )
+ )
+ }
+ j--
+ }
+ s = s.append(" (R^2 = " + String.format("%.3f", R2()) + ")")
+
+ // replace "+ -2n" with "- 2n"
+ return s.toString().replace("+ -", "- ")
+ }
+
+ /** Compare lexicographically. */
+ override fun compareTo(other: Regression?): Int {
+ val EPSILON = 1E-5
+ val maxDegree = max(degree().toDouble(), other!!.degree().toDouble()).toInt()
+ for (j in maxDegree downTo 0) {
+ var term1 = 0.0
+ var term2 = 0.0
+ if (degree() >= j) term1 = beta(j)
+ if (other.degree() >= j) term2 = other.beta(j)
+ if (abs(term1) < EPSILON) term1 = 0.0
+ if (abs(term2) < EPSILON) term2 = 0.0
+ if (term1 < term2) return -1 else if (term1 > term2) return +1
+ }
+ return 0
+ }
+}
\ No newline at end of file
diff --git a/src/main/kotlin/frc/team449/robot2023/commands/driveAlign/OrbitAlign.kt b/src/main/kotlin/frc/team449/robot2023/commands/driveAlign/OrbitAlign.kt
new file mode 100644
index 0000000..bc5aaa5
--- /dev/null
+++ b/src/main/kotlin/frc/team449/robot2023/commands/driveAlign/OrbitAlign.kt
@@ -0,0 +1,55 @@
+package frc.team449.robot2023.commands.driveAlign
+
+import edu.wpi.first.math.MathUtil
+import edu.wpi.first.math.controller.PIDController
+import edu.wpi.first.math.geometry.Translation2d
+import edu.wpi.first.math.kinematics.ChassisSpeeds
+import edu.wpi.first.wpilibj2.command.Command
+import frc.team449.control.DriveSubsystem
+import frc.team449.control.OI
+import frc.team449.robot2023.constants.RobotConstants
+import frc.team449.robot2023.constants.auto.AutoConstants
+
+/**
+ * @param drive The holonomic drive you want to align with
+ * @param point The point in 2d space you want the drivetrain to face towards
+ * @param headingPID The non-Profiled PID controller you want to use for fixing rotational error
+ */
+class OrbitAlign(
+ private val drive: DriveSubsystem,
+ private val oi: OI,
+ private val point: Translation2d,
+ private val headingPID: PIDController = PIDController(
+ AutoConstants.ORBIT_KP,
+ 0.0,
+ 0.0
+ )
+) : Command() {
+
+ init {
+ addRequirements(drive)
+ headingPID.enableContinuousInput(-Math.PI, Math.PI)
+ headingPID.setTolerance(0.015)
+ }
+
+ private var fieldToRobot = Translation2d()
+ private var robotToPoint = Translation2d()
+
+ override fun execute() {
+ fieldToRobot = drive.pose.translation
+ robotToPoint = point - fieldToRobot
+ headingPID.setpoint = robotToPoint.angle.radians
+
+ drive.set(
+ ChassisSpeeds(
+ oi.get().vxMetersPerSecond,
+ oi.get().vyMetersPerSecond,
+ MathUtil.clamp(headingPID.calculate(drive.heading.radians), -RobotConstants.MAX_ROT_SPEED, RobotConstants.MAX_ROT_SPEED)
+ )
+ )
+ }
+
+ override fun end(interrupted: Boolean) {
+ drive.stop()
+ }
+}
diff --git a/src/main/kotlin/frc/team449/robot2023/commands/driveAlign/ProfiledPoseAlign.kt b/src/main/kotlin/frc/team449/robot2023/commands/driveAlign/ProfiledPoseAlign.kt
new file mode 100644
index 0000000..297c0fe
--- /dev/null
+++ b/src/main/kotlin/frc/team449/robot2023/commands/driveAlign/ProfiledPoseAlign.kt
@@ -0,0 +1,119 @@
+package frc.team449.robot2023.commands.driveAlign
+
+import edu.wpi.first.math.controller.PIDController
+import edu.wpi.first.math.geometry.Pose2d
+import edu.wpi.first.math.geometry.Rotation2d
+import edu.wpi.first.math.kinematics.ChassisSpeeds
+import edu.wpi.first.math.trajectory.TrapezoidProfile
+import edu.wpi.first.wpilibj.Timer
+import edu.wpi.first.wpilibj2.command.Command
+import frc.team449.control.holonomic.SwerveDrive
+import frc.team449.robot2023.constants.RobotConstants
+import frc.team449.robot2023.constants.auto.AutoConstants
+import kotlin.math.PI
+import kotlin.math.hypot
+
+/**
+ * @param drive The holonomic drive you want to align with
+ * @param targetPose The pose you want to drive up to
+ * @param xPID The profiled PID controller with constraints you want to use for fixing X error
+ * @param yPID The profiled PID controller with constraints you want to use for fixing Y error
+ * @param headingPID The non-Profiled PID controller you want to use for fixing rotational error
+ * @param tolerance The allowed tolerance from the targetPose
+ */
+class ProfiledPoseAlign(
+ private val drive: SwerveDrive,
+ private val targetPose: Pose2d,
+ private val xSpeed: Double,
+ private val ySpeed: Double,
+ private val xPID: PIDController = PIDController(
+ AutoConstants.DEFAULT_X_KP,
+ 0.0,
+ 0.0
+ ),
+ private val yPID: PIDController = PIDController(
+ AutoConstants.DEFAULT_Y_KP,
+ 0.0,
+ 0.0
+ ),
+ private val headingPID: PIDController = PIDController(
+ AutoConstants.DEFAULT_ROTATION_KP,
+ 0.0,
+ 0.0
+ ),
+ private val xProfile: TrapezoidProfile = TrapezoidProfile(TrapezoidProfile.Constraints(RobotConstants.MAX_LINEAR_SPEED - 1.25, 2.25)),
+ private val yProfile: TrapezoidProfile = TrapezoidProfile(TrapezoidProfile.Constraints(RobotConstants.MAX_LINEAR_SPEED - 1.25, 2.25)),
+ private val tolerance: Pose2d = Pose2d(0.05, 0.05, Rotation2d(0.05)),
+ private val speedTol: Double = 0.05,
+ private val speedTolRot: Double = 0.05
+) : Command() {
+ init {
+ addRequirements(drive)
+ }
+
+ val timer = Timer()
+
+ override fun initialize() {
+ headingPID.enableContinuousInput(-PI, PI)
+
+ // Set tolerances from the given pose tolerance
+ xPID.setTolerance(tolerance.x)
+ yPID.setTolerance(tolerance.y)
+ headingPID.setTolerance(tolerance.rotation.radians)
+
+ headingPID.setpoint = targetPose.rotation.radians
+
+ timer.restart()
+ }
+
+ fun getTime(): Double {
+ return maxOf(xProfile.totalTime(), yProfile.totalTime(), 0.5)
+ }
+
+ override fun execute() {
+ val currTime = timer.get()
+
+ // Calculate the feedback for X, Y, and theta using their respective controllers
+
+ val xProfCalc = xProfile.calculate(
+ currTime,
+ TrapezoidProfile.State(targetPose.x, 0.0),
+ TrapezoidProfile.State(drive.pose.x, xSpeed)
+ )
+
+ val yProfCalc = yProfile.calculate(
+ currTime,
+ TrapezoidProfile.State(targetPose.y, 0.0),
+ TrapezoidProfile.State(drive.pose.y, ySpeed)
+ )
+
+ val xFeedback = xPID.calculate(drive.pose.x, xProfCalc.position)
+ val yFeedback = yPID.calculate(drive.pose.y, yProfCalc.position)
+ val headingFeedback = headingPID.calculate(drive.heading.radians)
+
+ drive.set(
+ ChassisSpeeds.fromFieldRelativeSpeeds(
+ xFeedback + xProfCalc.velocity,
+ yFeedback + yProfCalc.velocity,
+ headingFeedback,
+ drive.heading
+ )
+ )
+ }
+
+ override fun isFinished(): Boolean {
+ val currTime = timer.get()
+
+ return xPID.atSetpoint() && yPID.atSetpoint() && headingPID.atSetpoint() &&
+ xProfile.isFinished(currTime) && yProfile.isFinished(currTime) &&
+ hypot(
+ drive.currentSpeeds.vxMetersPerSecond,
+ drive.currentSpeeds.vyMetersPerSecond
+ ) < speedTol &&
+ drive.currentSpeeds.omegaRadiansPerSecond < speedTolRot
+ }
+
+ override fun end(interrupted: Boolean) {
+ drive.stop()
+ }
+}
diff --git a/src/main/kotlin/frc/team449/robot2023/commands/light/BlairChasing.kt b/src/main/kotlin/frc/team449/robot2023/commands/light/BlairChasing.kt
new file mode 100644
index 0000000..1cf81fb
--- /dev/null
+++ b/src/main/kotlin/frc/team449/robot2023/commands/light/BlairChasing.kt
@@ -0,0 +1,45 @@
+package frc.team449.robot2023.commands.light
+
+import edu.wpi.first.math.MathUtil
+import edu.wpi.first.wpilibj2.command.Command
+import frc.team449.robot2023.constants.subsystem.LightConstants
+import frc.team449.robot2023.subsystems.light.Light
+
+/** Description: Have a linear transition from white to red */
+class BlairChasing(
+ private val led: Light
+) : Command() {
+
+ init {
+ addRequirements(led)
+ }
+
+ override fun runsWhenDisabled(): Boolean {
+ return true
+ }
+
+ private var firstSaturation1 = 0.0
+ private var firstSaturation2 = 0.0
+
+ override fun execute() {
+ for (i in LightConstants.SECTION_1_START..LightConstants.SECTION_1_END) {
+ // This number is related to how many lights will show up between the high and low intensity (which technically also affects how fast itll cycle)
+ val saturation = MathUtil.inputModulus(firstSaturation1 + i * 200.0 / led.buffer.length, 0.0, 255.0)
+ led.setHSV(i, 0, saturation.toInt(), 255)
+
+ // The i * 255.0 relates to how fast it will cycle in between the high and low intensity
+ firstSaturation1 += 0.135
+ firstSaturation1 = MathUtil.inputModulus(firstSaturation1, 0.0, 255.0)
+ }
+
+ for (i in LightConstants.SECTION_2_START..LightConstants.SECTION_2_END) {
+ // This number is related to how many lights will show up between the high and low intensity (which technically also affects how fast itll cycle)
+ val saturation = MathUtil.inputModulus(firstSaturation2 + (i - LightConstants.SECTION_2_START) * 200.0 / led.buffer.length, 0.0, 255.0)
+ led.setHSV(i, 0, saturation.toInt(), 255)
+
+ // The i * 255.0 relates to how fast it will cycle in between the high and low intensity
+ firstSaturation2 += 0.135
+ firstSaturation2 = MathUtil.inputModulus(firstSaturation2, 0.0, 255.0)
+ }
+ }
+}
diff --git a/src/main/kotlin/frc/team449/robot2023/commands/light/BreatheHue.kt b/src/main/kotlin/frc/team449/robot2023/commands/light/BreatheHue.kt
new file mode 100644
index 0000000..4fc7a9f
--- /dev/null
+++ b/src/main/kotlin/frc/team449/robot2023/commands/light/BreatheHue.kt
@@ -0,0 +1,47 @@
+package frc.team449.robot2023.commands.light
+
+import edu.wpi.first.math.MathUtil
+import edu.wpi.first.wpilibj2.command.Command
+import frc.team449.robot2023.constants.subsystem.LightConstants
+import frc.team449.robot2023.subsystems.light.Light
+
+/** Description: Have a linear transition from a darker to brighter specified hue */
+class BreatheHue(
+ private val led: Light,
+ private val hue: Int
+) : Command() {
+
+ init {
+ addRequirements(led)
+ }
+
+ override fun runsWhenDisabled(): Boolean {
+ return true
+ }
+
+ private var firstIntensity1 = 175.0
+ private var firstIntensity2 = 175.0
+
+
+ override fun execute() {
+ for (i in LightConstants.SECTION_1_START..LightConstants.SECTION_1_END) {
+ // This number is related to how many lights will show up between the high and low intensity
+ val intensity = MathUtil.inputModulus(firstIntensity1 + i * 32.5 / led.buffer.length, 125.0, 255.0)
+ led.setHSV(i, hue, 255, intensity.toInt())
+
+ // The i * 255.0 relates to how fast it will cycle in between the high and low intensity
+ firstIntensity1 += 0.1
+ firstIntensity1 = MathUtil.inputModulus(firstIntensity1, 125.0, 255.0)
+ }
+
+ for (i in LightConstants.SECTION_2_START..LightConstants.SECTION_2_END) {
+ // This number is related to how many lights will show up between the high and low intensity
+ val intensity = MathUtil.inputModulus(firstIntensity2 + (i - LightConstants.SECTION_2_START) * 32.5 / led.buffer.length, 125.0, 255.0)
+ led.setHSV(i, hue, 255, intensity.toInt())
+
+ // The i * 255.0 relates to how fast it will cycle in between the high and low intensity
+ firstIntensity2 += 0.1
+ firstIntensity2 = MathUtil.inputModulus(firstIntensity2, 125.0, 255.0)
+ }
+ }
+}
diff --git a/src/main/kotlin/frc/team449/robot2023/commands/light/Crazy.kt b/src/main/kotlin/frc/team449/robot2023/commands/light/Crazy.kt
new file mode 100644
index 0000000..ee3ba56
--- /dev/null
+++ b/src/main/kotlin/frc/team449/robot2023/commands/light/Crazy.kt
@@ -0,0 +1,26 @@
+package frc.team449.robot2023.commands.light
+
+import edu.wpi.first.wpilibj2.command.Command
+import frc.team449.robot2023.subsystems.light.Light
+import kotlin.random.Random
+
+/** Description: Have a random color set for every led every 20ms
+ * a.k.a. you go blind */
+class Crazy(
+ private val led: Light
+) : Command() {
+
+ init {
+ addRequirements(led)
+ }
+
+ override fun runsWhenDisabled(): Boolean {
+ return true
+ }
+
+ override fun execute() {
+ for (i in 0 until led.buffer.length) {
+ led.setRGB(i, Random.nextInt(0, 256), Random.nextInt(0, 256), Random.nextInt(0, 256))
+ }
+ }
+}
diff --git a/src/main/kotlin/frc/team449/robot2023/commands/light/PickupBlink.kt b/src/main/kotlin/frc/team449/robot2023/commands/light/PickupBlink.kt
new file mode 100644
index 0000000..078315d
--- /dev/null
+++ b/src/main/kotlin/frc/team449/robot2023/commands/light/PickupBlink.kt
@@ -0,0 +1,33 @@
+package frc.team449.robot2023.commands.light
+
+import edu.wpi.first.wpilibj2.command.Command
+import edu.wpi.first.wpilibj2.command.InstantCommand
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup
+import edu.wpi.first.wpilibj2.command.WaitCommand
+import frc.team449.robot2023.subsystems.light.Light
+
+/** Description: Blink a certain color 5 times */
+class PickupBlink {
+ fun blinkGreen(light: Light): Command {
+ val cmdGroup = SequentialCommandGroup()
+
+ cmdGroup.addRequirements(light)
+
+ for (x in 0 until 5) {
+ cmdGroup.addCommands(InstantCommand({ setColor(light, 0, 255, 0) }))
+ cmdGroup.addCommands(WaitCommand(0.1))
+ cmdGroup.addCommands(InstantCommand({ setColor(light, 0, 0, 0) }))
+ cmdGroup.addCommands(WaitCommand(0.1))
+ }
+
+ cmdGroup.ignoringDisable(true)
+
+ return cmdGroup
+ }
+
+ private fun setColor(led: Light, r: Int, g: Int, b: Int) {
+ for (i in 0 until led.buffer.length) {
+ led.setRGB(i, r, g, b)
+ }
+ }
+}
diff --git a/src/main/kotlin/frc/team449/robot2023/commands/light/Rainbow.kt b/src/main/kotlin/frc/team449/robot2023/commands/light/Rainbow.kt
new file mode 100644
index 0000000..cc822cd
--- /dev/null
+++ b/src/main/kotlin/frc/team449/robot2023/commands/light/Rainbow.kt
@@ -0,0 +1,38 @@
+package frc.team449.robot2023.commands.light
+
+import edu.wpi.first.math.MathUtil
+import edu.wpi.first.wpilibj2.command.Command
+import frc.team449.robot2023.constants.subsystem.LightConstants
+import frc.team449.robot2023.subsystems.light.Light
+
+class Rainbow(
+ private val led: Light
+) : Command() {
+
+ init {
+ addRequirements(led)
+ }
+
+ override fun runsWhenDisabled(): Boolean {
+ return true
+ }
+
+ private var firstHue = 0.0
+
+ override fun execute() {
+ for (i in LightConstants.SECTION_1_START..LightConstants.SECTION_1_END) {
+ val hue = MathUtil.inputModulus(firstHue + i * 180 / (LightConstants.SECTION_1_END - LightConstants.SECTION_1_START), 0.0, 180.0)
+
+ led.setHSV(i, hue.toInt(), 255, 255)
+ }
+
+ for (i in LightConstants.SECTION_2_START..LightConstants.SECTION_2_END) {
+ val hue = MathUtil.inputModulus(firstHue + (i - LightConstants.SECTION_2_START) * 180 / (LightConstants.SECTION_2_END - LightConstants.SECTION_2_START), 0.0, 180.0)
+
+ led.setHSV(i, hue.toInt(), 255, 255)
+ }
+
+ firstHue += 6.15
+ firstHue = MathUtil.inputModulus(firstHue, 0.0, 180.0)
+ }
+}
diff --git a/src/main/kotlin/frc/team449/robot2023/commands/light/SolidColor.kt b/src/main/kotlin/frc/team449/robot2023/commands/light/SolidColor.kt
new file mode 100644
index 0000000..0449d6a
--- /dev/null
+++ b/src/main/kotlin/frc/team449/robot2023/commands/light/SolidColor.kt
@@ -0,0 +1,27 @@
+package frc.team449.robot2023.commands.light
+
+import edu.wpi.first.wpilibj2.command.Command
+import frc.team449.robot2023.subsystems.light.Light
+
+/** Description: Have a solid color */
+class SolidColor(
+ private val led: Light,
+ private val r: Int,
+ private val g: Int,
+ private val b: Int
+) : Command() {
+
+ init {
+ addRequirements(led)
+ }
+
+ override fun runsWhenDisabled(): Boolean {
+ return true
+ }
+
+ override fun execute() {
+ for (i in 0 until led.buffer.length) {
+ led.setRGB(i, r, g, b)
+ }
+ }
+}
diff --git a/src/main/kotlin/frc/team449/robot2023/constants/MotorConstants.kt b/src/main/kotlin/frc/team449/robot2023/constants/MotorConstants.kt
new file mode 100644
index 0000000..e760563
--- /dev/null
+++ b/src/main/kotlin/frc/team449/robot2023/constants/MotorConstants.kt
@@ -0,0 +1,12 @@
+package frc.team449.robot2023.constants
+
+import edu.wpi.first.math.util.Units
+
+object MotorConstants {
+ /** NEO characteristics */
+ const val NOMINAL_VOLTAGE = 12.0
+ const val STALL_TORQUE = 3.36
+ const val STALL_CURRENT = 166.0
+ const val FREE_CURRENT = 1.3
+ val FREE_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(5676.0)
+}
diff --git a/src/main/kotlin/frc/team449/robot2023/constants/RobotConstants.kt b/src/main/kotlin/frc/team449/robot2023/constants/RobotConstants.kt
new file mode 100644
index 0000000..380f395
--- /dev/null
+++ b/src/main/kotlin/frc/team449/robot2023/constants/RobotConstants.kt
@@ -0,0 +1,61 @@
+package frc.team449.robot2023.constants
+
+import edu.wpi.first.math.controller.PIDController
+import edu.wpi.first.math.geometry.Pose2d
+import edu.wpi.first.math.geometry.Rotation2d
+import edu.wpi.first.math.system.plant.DCMotor
+import edu.wpi.first.math.util.Units
+import frc.team449.robot2023.constants.drives.SwerveConstants
+import kotlin.math.PI
+
+object RobotConstants {
+
+ /** Other CAN ID */
+ const val PDH_CAN = 1
+
+ /** Controller Configurations */
+ const val ROT_RATE_LIMIT = 4.0 * PI
+ const val NEG_ROT_RATE_LIM = -8.0 * PI
+ const val TRANSLATION_DEADBAND = .15
+ const val ROTATION_DEADBAND = .15
+
+ /** In kilograms, include bumpers and battery and all */
+ const val ROBOT_WEIGHT = 55
+
+ /** Drive configuration */
+ const val MAX_LINEAR_SPEED = SwerveConstants.MAX_ATTAINABLE_MK4I_SPEED // m/s
+ const val MAX_ROT_SPEED = PI // rad/s
+ val MAX_ACCEL = 4 * DCMotor(
+ MotorConstants.NOMINAL_VOLTAGE,
+ MotorConstants.STALL_TORQUE * SwerveConstants.EFFICIENCY,
+ MotorConstants.STALL_CURRENT,
+ MotorConstants.FREE_CURRENT,
+ MotorConstants.FREE_SPEED,
+ 1
+ ).getTorque(55.0) /
+ (Units.inchesToMeters(2.0) * ROBOT_WEIGHT * SwerveConstants.DRIVE_GEARING) // m/s/s
+
+ val INITIAL_POSE = Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0))
+
+
+ init {
+ println("Max Accel $MAX_ACCEL")
+ }
+
+ const val LOOP_TIME = 0.020
+
+ /** PID controller for Orthogonal turning */
+ val ORTHOGONAL_CONTROLLER = PIDController(
+ 3.0,
+ 0.0,
+ 0.0
+ )
+
+ const val ALIGN_ROT_SPEED = 3 * PI / 2
+
+ val IR_CHANNEL = 15
+
+ // Robot dimensions (INCLUDING BUMPERS)
+ val ROBOT_WIDTH = Units.inchesToMeters(27.0 + 3.25 * 2)
+ val ROBOT_LENGTH = Units.inchesToMeters(30.0 + 3.25 * 2)
+}
diff --git a/src/main/kotlin/frc/team449/robot2023/constants/auto/AutoConstants.kt b/src/main/kotlin/frc/team449/robot2023/constants/auto/AutoConstants.kt
new file mode 100644
index 0000000..55f3ee0
--- /dev/null
+++ b/src/main/kotlin/frc/team449/robot2023/constants/auto/AutoConstants.kt
@@ -0,0 +1,12 @@
+package frc.team449.robot2023.constants.auto
+
+import kotlin.math.PI
+
+object AutoConstants {
+ /** PID gains */
+ const val DEFAULT_X_KP = 1.875
+ const val DEFAULT_Y_KP = 1.875
+ const val DEFAULT_ROTATION_KP = 2.0
+
+ const val ORBIT_KP = 2 * PI
+}
diff --git a/src/main/kotlin/frc/team449/robot2023/constants/drives/DifferentialConstants.kt b/src/main/kotlin/frc/team449/robot2023/constants/drives/DifferentialConstants.kt
new file mode 100644
index 0000000..0092955
--- /dev/null
+++ b/src/main/kotlin/frc/team449/robot2023/constants/drives/DifferentialConstants.kt
@@ -0,0 +1,53 @@
+package frc.team449.robot2023.constants.drives
+
+import edu.wpi.first.math.controller.DifferentialDriveFeedforward
+import edu.wpi.first.math.util.Units
+import edu.wpi.first.wpilibj.Encoder
+import java.lang.Math.PI
+
+/** Constants for a differential drivetrain */
+object DifferentialConstants {
+
+ /** Drive motor CAN ID */
+ const val DRIVE_MOTOR_L = 2
+ const val DRIVE_MOTOR_L1 = 4
+ const val DRIVE_MOTOR_L2 = 3
+ const val DRIVE_MOTOR_R = 1
+ const val DRIVE_MOTOR_R1 = 11
+ const val DRIVE_MOTOR_R2 = 7
+
+ /** Angular feed forward gains. */
+ const val DRIVE_ANGLE_FF_KS = 0.20112
+ const val DRIVE_ANGLE_FF_KV = 10.05
+ const val DRIVE_ANGLE_FF_KA = 0.505
+
+ /** Control Constants */
+ const val DRIVE_KP = .0
+ const val DRIVE_KI = .0
+ const val DRIVE_KD = .0
+ const val DRIVE_FF_KS = 0.1908
+ const val DRIVE_FF_KV = 2.5406
+ const val DRIVE_FF_KA = 0.44982
+
+ /** Encoder Characteristics */
+ val DRIVE_ENC_RIGHT = Encoder(4, 5)
+ val DRIVE_ENC_LEFT = Encoder(6, 7)
+ const val NEO_ENCODER_CPR = 1
+ const val DRIVE_EXT_ENC_CPR = 256
+
+ /** Drive Characteristics */
+ const val DRIVE_GEARING = 1.0
+ val DRIVE_WHEEL_RADIUS = Units.inchesToMeters(2.0)
+ val DRIVE_UPR = 2 * PI * DRIVE_WHEEL_RADIUS
+ const val DRIVE_CURRENT_LIM = 35
+ const val DRIVE_ENC_VEL_THRESHOLD = 999999.0
+ const val TRACK_WIDTH = .615 // m
+
+ val DRIVE_FEED_FORWARD = DifferentialDriveFeedforward(
+ DRIVE_FF_KV,
+ DRIVE_FF_KA,
+ DRIVE_ANGLE_FF_KV,
+ DRIVE_ANGLE_FF_KA,
+ TRACK_WIDTH
+ )
+}
diff --git a/src/main/kotlin/frc/team449/robot2023/constants/drives/MecanumConstants.kt b/src/main/kotlin/frc/team449/robot2023/constants/drives/MecanumConstants.kt
new file mode 100644
index 0000000..24ac303
--- /dev/null
+++ b/src/main/kotlin/frc/team449/robot2023/constants/drives/MecanumConstants.kt
@@ -0,0 +1,30 @@
+package frc.team449.robot2023.constants.drives
+
+import edu.wpi.first.math.util.Units
+
+object MecanumConstants {
+
+ /** Drive motor ports */
+ const val DRIVE_MOTOR_FL = 1
+ const val DRIVE_MOTOR_FR = 2
+ const val DRIVE_MOTOR_BL = 3
+ const val DRIVE_MOTOR_BR = 4
+
+ /** Feed forward values for driving each module */
+ const val DRIVE_KS = 0.16475
+ const val DRIVE_KV = 2.0909
+ const val DRIVE_KA = 0.29862
+
+ /** PID gains for driving each module*/
+ const val DRIVE_KP = 0.35
+ const val DRIVE_KI = 0.0
+ const val DRIVE_KD = 0.0
+
+ /** Drive configuration */
+ const val DRIVE_GEARING = 1 / 8.0
+ val DRIVE_UPR = Math.PI * Units.inchesToMeters(6.0)
+ const val MAX_ATTAINABLE_WHEEL_SPEED = (12 - DRIVE_KS) / DRIVE_KV
+ val WHEELBASE = Units.inchesToMeters(21.426)
+ val TRACKWIDTH = Units.inchesToMeters(21.000)
+ const val CURRENT_LIM = 40
+}
diff --git a/src/main/kotlin/frc/team449/robot2023/constants/drives/SwerveConstants.kt b/src/main/kotlin/frc/team449/robot2023/constants/drives/SwerveConstants.kt
new file mode 100644
index 0000000..3c2a48b
--- /dev/null
+++ b/src/main/kotlin/frc/team449/robot2023/constants/drives/SwerveConstants.kt
@@ -0,0 +1,61 @@
+package frc.team449.robot2023.constants.drives
+
+import edu.wpi.first.math.util.Units
+
+object SwerveConstants {
+ const val EFFICIENCY = 0.95
+
+ /** Drive motor ports */
+ const val DRIVE_MOTOR_FL = 11
+ const val DRIVE_MOTOR_FR = 17
+ const val DRIVE_MOTOR_BL = 38
+ const val DRIVE_MOTOR_BR = 22
+ const val TURN_MOTOR_FL = 20
+ const val TURN_MOTOR_FR = 3
+ const val TURN_MOTOR_BL = 41
+ const val TURN_MOTOR_BR = 45
+
+ /** Turning encoder channels */
+ const val TURN_ENC_CHAN_FL = 6
+ const val TURN_ENC_CHAN_FR = 9
+ const val TURN_ENC_CHAN_BL = 5
+ const val TURN_ENC_CHAN_BR = 8
+
+ /** Offsets for the absolute encoders in rotations */
+ const val TURN_ENC_OFFSET_FL = 0.396987
+ const val TURN_ENC_OFFSET_FR = 0.389095
+ const val TURN_ENC_OFFSET_BL = 0.787267 - 0.5
+ const val TURN_ENC_OFFSET_BR = 0.215026
+
+ /** PID gains for turning each module */
+ const val TURN_KP = 0.85
+ const val TURN_KI = 0.0
+ const val TURN_KD = 0.0
+
+ /** Feed forward values for driving each module */
+ const val DRIVE_KS = 0.2491250419322037
+ const val DRIVE_KV = 2.352910954352485
+ const val DRIVE_KA = 0.42824
+
+ // TODO: Figure out this value
+ const val STEER_KS = 1.0
+
+ /** PID gains for driving each module*/
+ const val DRIVE_KP = 0.4
+ const val DRIVE_KI = 0.0
+ const val DRIVE_KD = 0.0
+
+ /** Drive configuration */
+ const val DRIVE_GEARING = 1 / 6.12
+ const val DRIVE_UPR = 0.31818905832
+ const val TURN_UPR = 2 * Math.PI
+ const val MAX_ATTAINABLE_MK4I_SPEED = (12 - DRIVE_KS) / DRIVE_KV
+ const val DRIVE_CURRENT_LIM = 55
+ const val STEERING_CURRENT_LIM = 40
+ const val JOYSTICK_FILTER_ORDER = 2
+
+ /** Wheelbase = wheel-to-wheel distance from front to back of the robot */
+ /** Trackwidth = wheel-to-wheel distance from side to side of the robot */
+ val WHEELBASE = Units.inchesToMeters(24.75) // ex. FL to BL
+ val TRACKWIDTH = Units.inchesToMeters(21.75) // ex. BL to BR
+}
diff --git a/src/main/kotlin/frc/team449/robot2023/constants/field/FieldConstants.kt b/src/main/kotlin/frc/team449/robot2023/constants/field/FieldConstants.kt
new file mode 100644
index 0000000..a6071ba
--- /dev/null
+++ b/src/main/kotlin/frc/team449/robot2023/constants/field/FieldConstants.kt
@@ -0,0 +1,43 @@
+package frc.team449.robot2023.constants.field
+
+import edu.wpi.first.math.geometry.Pose2d
+import edu.wpi.first.math.geometry.Rotation2d
+import edu.wpi.first.math.util.Units
+import frc.team449.robot2023.constants.RobotConstants
+
+object FieldConstants {
+ val fieldLength = Units.feetToMeters(54.0)
+ val fieldWidth = Units.feetToMeters(27.0)
+
+ val wallNodeY = Units.inchesToMeters(20.19)
+ val nodeSeparationY = Units.inchesToMeters(22.0)
+
+ val nodeX = Units.inchesToMeters(54.05) + RobotConstants.ROBOT_LENGTH / 2
+
+ val midNodeFromEdge = Units.inchesToMeters(22.7)
+ val highNodeFromEdge = Units.inchesToMeters(39.73)
+
+ val PlacementPositions: Map = mapOf(
+ TargetPosition.Position1 to Pose2d(nodeX, wallNodeY, Rotation2d()),
+ TargetPosition.Position2 to Pose2d(nodeX, wallNodeY + nodeSeparationY, Rotation2d()),
+ TargetPosition.Position3 to Pose2d(nodeX, wallNodeY + 2 * nodeSeparationY, Rotation2d()),
+ TargetPosition.Position4 to Pose2d(nodeX, wallNodeY + 3 * nodeSeparationY, Rotation2d()),
+ TargetPosition.Position5 to Pose2d(nodeX, wallNodeY + 4 * nodeSeparationY, Rotation2d()),
+ TargetPosition.Position6 to Pose2d(nodeX, wallNodeY + 5 * nodeSeparationY, Rotation2d()),
+ TargetPosition.Position7 to Pose2d(nodeX, wallNodeY + 6 * nodeSeparationY, Rotation2d()),
+ TargetPosition.Position8 to Pose2d(nodeX, wallNodeY + 7 * nodeSeparationY, Rotation2d()),
+ TargetPosition.Position9 to Pose2d(nodeX, wallNodeY + 8 * nodeSeparationY, Rotation2d())
+ )
+
+ enum class TargetPosition {
+ Position1,
+ Position2,
+ Position3,
+ Position4,
+ Position5,
+ Position6,
+ Position7,
+ Position8,
+ Position9
+ }
+}
diff --git a/src/main/kotlin/frc/team449/robot2023/constants/subsystem/LightConstants.kt b/src/main/kotlin/frc/team449/robot2023/constants/subsystem/LightConstants.kt
new file mode 100644
index 0000000..7576685
--- /dev/null
+++ b/src/main/kotlin/frc/team449/robot2023/constants/subsystem/LightConstants.kt
@@ -0,0 +1,11 @@
+package frc.team449.robot2023.constants.subsystem
+
+object LightConstants {
+ const val LIGHT_PORT = 9
+ const val LIGHT_LENGTH = 24
+
+ const val SECTION_1_START = 0
+ const val SECTION_1_END = 11
+ const val SECTION_2_START = 12
+ const val SECTION_2_END = 23
+}
diff --git a/src/main/kotlin/frc/team449/robot2023/constants/vision/VisionConstants.kt b/src/main/kotlin/frc/team449/robot2023/constants/vision/VisionConstants.kt
new file mode 100644
index 0000000..2f836b7
--- /dev/null
+++ b/src/main/kotlin/frc/team449/robot2023/constants/vision/VisionConstants.kt
@@ -0,0 +1,93 @@
+package frc.team449.robot2023.constants.vision
+
+import edu.wpi.first.apriltag.AprilTag
+import edu.wpi.first.apriltag.AprilTagFieldLayout
+import edu.wpi.first.math.MatBuilder
+import edu.wpi.first.math.Matrix
+import edu.wpi.first.math.Nat
+import edu.wpi.first.math.geometry.Pose3d
+import edu.wpi.first.math.geometry.Rotation3d
+import edu.wpi.first.math.geometry.Transform3d
+import edu.wpi.first.math.geometry.Translation3d
+import edu.wpi.first.math.numbers.N1
+import edu.wpi.first.math.numbers.N3
+import edu.wpi.first.math.util.Units
+import edu.wpi.first.wpilibj.Filesystem
+import frc.team449.control.vision.VisionSubsystem
+import org.photonvision.estimation.TargetModel
+import org.photonvision.simulation.VisionSystemSim
+
+/** Constants that have anything to do with vision */
+object VisionConstants {
+ /** How the tags are laid out on the field (their locations and ids) */
+ private val TEST_TAG_LAYOUT = AprilTagFieldLayout(
+ listOf(
+ AprilTag(3, Pose3d())
+ ),
+ 16.4846,
+ 8.1026
+ )
+
+ /** WPILib's AprilTagFieldLayout for the 2023 Charged Up Game */
+ val TAG_LAYOUT: AprilTagFieldLayout = AprilTagFieldLayout(
+ Filesystem.getDeployDirectory().absolutePath.plus("/vision/Bunnybots2023.json")
+ )
+
+// AprilTagFieldLayout.loadFromResource(
+// AprilTagFields.k2023ChargedUp.m_resourceFile
+// )
+
+ /** Robot to Camera distance */
+ val robotToCamera = Transform3d(
+ Translation3d(Units.inchesToMeters(-11.48657), Units.inchesToMeters(0.0), Units.inchesToMeters(8.3416)),
+ Rotation3d(0.0, Units.degreesToRadians(15.0), Units.degreesToRadians(180.0))
+ )
+
+ val TAG_MODEL = TargetModel(
+ Units.inchesToMeters(6.5),
+ Units.inchesToMeters(6.5)
+ )
+
+ /** Filtering Constants */
+ const val MAX_AMBIGUITY = 0.325
+ const val MAX_DISTANCE_SINGLE_TAG = 3.45
+ const val MAX_DISTANCE_MULTI_TAG = 4.5
+ const val SINGLE_TAG_HEADING_MAX_DEV_DEG = 5.0
+ const val MAX_HEIGHT_ERR_METERS = 0.075
+ const val TAG_MULTIPLIER = 0.5
+
+ /** Std Dev Calculation Constants */
+ const val ORDER = 2
+ const val PROPORTION = 1 / 25
+
+ val VISION_SIM = VisionSystemSim(
+ "main"
+ )
+
+ /** Vision Sim Setup Constants */
+ const val SIM_FPS = 25.0
+ const val SIM_CAMERA_HEIGHT_PX = 720
+ const val SIM_CAMERA_WIDTH_PX = 1280
+ const val SIM_FOV_DEG = 75.0
+ const val SIM_CALIB_AVG_ERR_PX = 0.35
+ const val SIM_CALIB_ERR_STDDEV_PX = 0.10
+ const val SIM_AVG_LATENCY = 50.0
+ const val SIM_STDDEV_LATENCY = 10.0
+ const val ENABLE_WIREFRAME = true
+
+
+ /** List of cameras that we want to use */
+ val ESTIMATORS: ArrayList = arrayListOf(
+ VisionSubsystem(
+ "arducam",
+ TAG_LAYOUT,
+ robotToCamera,
+ VISION_SIM
+ )
+ )
+
+ val ENCODER_TRUST: Matrix = MatBuilder(Nat.N3(), Nat.N1()).fill(.085, .085, .015)
+ val SINGLE_TAG_TRUST: Matrix = MatBuilder(Nat.N3(), Nat.N1()).fill(.125, .125, .80)
+ val MULTI_TAG_TRUST: Matrix = MatBuilder(Nat.N3(), Nat.N1()).fill(.0275, .0275, .30)
+
+}
diff --git a/src/main/kotlin/frc/team449/robot2023/subsystems/ControllerBindings.kt b/src/main/kotlin/frc/team449/robot2023/subsystems/ControllerBindings.kt
new file mode 100644
index 0000000..66855b7
--- /dev/null
+++ b/src/main/kotlin/frc/team449/robot2023/subsystems/ControllerBindings.kt
@@ -0,0 +1,53 @@
+package frc.team449.robot2023.subsystems
+
+import edu.wpi.first.math.geometry.Rotation2d
+import edu.wpi.first.wpilibj.XboxController
+import edu.wpi.first.wpilibj2.command.*
+import edu.wpi.first.wpilibj2.command.button.JoystickButton
+import edu.wpi.first.wpilibj2.command.button.Trigger
+import frc.team449.robot2023.Robot
+import frc.team449.robot2023.commands.characterization.Characterization
+import frc.team449.robot2023.constants.RobotConstants
+import kotlin.math.PI
+
+
+class ControllerBindings(
+ private val driveController: XboxController,
+ private val mechanismController: XboxController,
+ private val robot: Robot
+) {
+
+ fun bindButtons() {
+ // slow drive
+ Trigger { driveController.rightTriggerAxis >= .75 }.onTrue(
+ InstantCommand({ robot.drive.maxLinearSpeed = 1.0 }).andThen(
+ InstantCommand({ robot.drive.maxRotSpeed = PI / 4 })
+ )
+ ).onFalse(
+ InstantCommand({ robot.drive.maxLinearSpeed = RobotConstants.MAX_LINEAR_SPEED }).andThen(
+ InstantCommand({ robot.drive.maxRotSpeed = RobotConstants.MAX_ROT_SPEED })
+ )
+ )
+
+ // reset gyro
+ JoystickButton(driveController, XboxController.Button.kStart.value).onTrue(
+ InstantCommand({
+ robot.drive.heading = Rotation2d()
+ })
+ )
+
+ // characterize
+ JoystickButton(driveController, XboxController.Button.kA.value).onTrue(
+ Characterization(
+ robot.drive,
+ true,
+ "swerve drive",
+ robot.drive::setVoltage,
+ robot.drive::getModuleVel
+ ).withInterruptBehavior(Command.InterruptionBehavior.kCancelIncoming)
+ ).onFalse(
+ robot.driveCommand
+ )
+
+ }
+}
diff --git a/src/main/kotlin/frc/team449/robot2023/subsystems/light/Light.kt b/src/main/kotlin/frc/team449/robot2023/subsystems/light/Light.kt
new file mode 100644
index 0000000..4ca5771
--- /dev/null
+++ b/src/main/kotlin/frc/team449/robot2023/subsystems/light/Light.kt
@@ -0,0 +1,88 @@
+package frc.team449.robot2023.subsystems.light
+
+import edu.wpi.first.wpilibj.AddressableLED
+import edu.wpi.first.wpilibj.AddressableLEDBuffer
+import edu.wpi.first.wpilibj.RobotBase
+import edu.wpi.first.wpilibj2.command.SubsystemBase
+import frc.team449.robot2023.constants.subsystem.LightConstants
+
+/**
+ * Controls an LED strip.
+ * @param port The PWM port of the LED strip.
+ * @param length The length of the LED strip.
+ */
+
+class Light(
+ port: Int,
+ length: Int
+) : SubsystemBase() {
+
+ private val lightStrip = AddressableLED(port)
+ val buffer = AddressableLEDBuffer(length)
+
+ init {
+ lightStrip.setLength(buffer.length)
+ lightStrip.setData(buffer)
+ lightStrip.start()
+ }
+
+ override fun periodic() {
+ lightStrip.setData(buffer)
+ }
+
+ /** Copy the AddressableLEDBuffer setHsv() except to support
+ * GRB LED strips */
+ fun setHSV(index: Int, h: Int, s: Int, v: Int) {
+ if (RobotBase.isReal()) {
+ if (s == 0) {
+ buffer.setRGB(index, v, v, v)
+ return
+ }
+
+ // Difference between highest and lowest value of any rgb component
+ val chroma = s * v / 255
+
+ // Because hue is 0-180 rather than 0-360 use 30 not 60
+ val region = h / 30 % 6
+
+ // Remainder converted from 0-30 to 0-255
+ val remainder = Math.round(h % 30 * (255 / 30.0)).toInt()
+
+ // Value of the lowest rgb component
+ val m = v - chroma
+
+ // Goes from 0 to chroma as hue increases
+ val X = chroma * remainder shr 8
+ when (region) {
+ 0 -> buffer.setRGB(index, X + m, v, m)
+ 1 -> buffer.setRGB(index, v, v - X, m)
+ 2 -> buffer.setRGB(index, v, m, X + m)
+ 3 -> buffer.setRGB(index, v - X, m, v)
+ 4 -> buffer.setRGB(index, m, X + m, v)
+ else -> buffer.setRGB(index, m, v, v - X)
+ }
+ } else {
+ buffer.setHSV(index, h, s, v)
+ }
+ }
+
+ /** Copy the AddressableLEDBuffer setRGB() except to support
+ * GRB LED strips */
+ fun setRGB(index: Int, r: Int, g: Int, b: Int) {
+ if (RobotBase.isReal()) {
+ buffer.setRGB(index, g, r, b)
+ } else {
+ buffer.setRGB(index, r, g, b)
+ }
+ }
+
+ companion object {
+ /** Create an LED strip controller using [LightConstants]. */
+ fun createLight(): Light {
+ return Light(
+ LightConstants.LIGHT_PORT,
+ LightConstants.LIGHT_LENGTH
+ )
+ }
+ }
+}
diff --git a/src/main/kotlin/frc/team449/system/AHRS.kt b/src/main/kotlin/frc/team449/system/AHRS.kt
new file mode 100644
index 0000000..f10503f
--- /dev/null
+++ b/src/main/kotlin/frc/team449/system/AHRS.kt
@@ -0,0 +1,83 @@
+package frc.team449.system
+
+import edu.wpi.first.math.filter.LinearFilter
+import edu.wpi.first.math.geometry.Rotation2d
+import edu.wpi.first.wpilibj.SPI
+import edu.wpi.first.wpilibj.Timer
+import edu.wpi.first.wpilibj.simulation.SimDeviceSim
+import frc.team449.util.simBooleanProp
+import frc.team449.util.simDoubleProp
+
+class AHRS(
+ private val navx: com.kauailabs.navx.frc.AHRS
+) {
+
+ var prevPos = Double.NaN
+ var prevTime = Double.NaN
+
+ private val filter = LinearFilter.movingAverage(5)
+
+ /** The current reading of the gyro with the offset included */
+ val heading: Rotation2d
+ get() {
+ return -Rotation2d.fromDegrees(navx.fusedHeading.toDouble())
+ }
+
+ val pitch: Rotation2d
+ get() {
+ return -Rotation2d.fromDegrees(navx.pitch.toDouble())
+ }
+
+ val roll: Rotation2d
+ get() {
+ return -Rotation2d.fromDegrees(navx.roll.toDouble())
+ }
+
+ fun angularXVel(): Double {
+ val currPos = navx.roll.toDouble()
+ val currTime = Timer.getFPGATimestamp()
+
+ val vel = if (prevPos.isNaN()) {
+ 0.0
+ } else {
+ val dt = currTime - prevTime
+ val dx = currPos - prevPos
+ dx / dt
+ }
+ this.prevTime = currTime
+ this.prevPos = currPos
+ return filter.calculate(vel)
+ }
+
+ constructor(
+ port: SPI.Port = SPI.Port.kMXP
+ ) : this(
+ com.kauailabs.navx.frc.AHRS(port)
+ )
+
+ fun calibrated(): Boolean {
+ return navx.isMagnetometerCalibrated
+ }
+
+ /**
+ * Used to set properties of an [AHRS] object during simulation. See
+ * https://pdocs.kauailabs.com/navx-mxp/softwa
+ * re/roborio-libraries/java/
+ *
+ * @param devName The name of the simulated device.
+ * @param index The NavX index.
+ */
+ class SimController(devName: String = "navX-Sensor", index: Int = 0) {
+ private val deviceSim = SimDeviceSim(devName, index)
+
+ var isConnected by simBooleanProp(deviceSim.getBoolean("Connected"))
+ var yaw by simDoubleProp(deviceSim.getDouble("Yaw"))
+ var pitch by simDoubleProp(deviceSim.getDouble("Pitch"))
+ var roll by simDoubleProp(deviceSim.getDouble("Roll"))
+ var compassHeading by simDoubleProp(deviceSim.getDouble("CompassHeading"))
+ var fusedHeading by simDoubleProp(deviceSim.getDouble("FusedHeading"))
+ var linearWorldAccelX by simDoubleProp(deviceSim.getDouble("LinearWorldAccelX"))
+ var linearWorldAccelY by simDoubleProp(deviceSim.getDouble("LinearWorldAccelX"))
+ var linearWorldAccelZ by simDoubleProp(deviceSim.getDouble("LinearWorldAccelY"))
+ }
+}
diff --git a/src/main/kotlin/frc/team449/system/SimBattery.kt b/src/main/kotlin/frc/team449/system/SimBattery.kt
new file mode 100644
index 0000000..463e17d
--- /dev/null
+++ b/src/main/kotlin/frc/team449/system/SimBattery.kt
@@ -0,0 +1,27 @@
+package frc.team449.system
+
+import edu.wpi.first.wpilibj.simulation.BatterySim
+import edu.wpi.first.wpilibj.simulation.RoboRioSim
+
+/**
+ * Used for simulating battery voltage and current based off drawn currents from other mechanisms
+ */
+class SimBattery {
+ private val currentDrawers = mutableListOf<() -> Double>()
+
+ /**
+ * Register a simulated mechanism drawing current
+ */
+ fun addCurrentDrawer(currentDrawer: () -> Double) {
+ currentDrawers.add(currentDrawer)
+ }
+
+ /**
+ * Update the simulation with a newly calculated battery current and voltage
+ */
+ fun update() {
+ val currents = currentDrawers.map { it() }
+ RoboRioSim.setVInCurrent(currents.sum())
+ RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(*currents.toDoubleArray()))
+ }
+}
diff --git a/src/main/kotlin/frc/team449/system/SparkUtil.kt b/src/main/kotlin/frc/team449/system/SparkUtil.kt
new file mode 100644
index 0000000..b62928d
--- /dev/null
+++ b/src/main/kotlin/frc/team449/system/SparkUtil.kt
@@ -0,0 +1,79 @@
+package frc.team449.system
+
+import com.revrobotics.*
+import edu.wpi.first.wpilibj.RobotController
+
+object SparkUtil {
+ fun applySparkSettings(
+
+ // spark max information
+ sparkMax: CANSparkMax,
+ enableBrakeMode: Boolean = true,
+ inverted: Boolean = false,
+ currentLimit: Int = 0,
+ enableVoltageComp: Boolean = false,
+ slaveSparks: Map = mapOf(),
+ controlFrameRateMillis: Int = -1,
+ statusFrameRatesMillis: Map = mapOf(),
+
+ // encoder information
+ encoder: MotorFeedbackSensor,
+ unitPerRotation: Double,
+ gearing: Double,
+ offset: Double = 0.0,
+ encInverted: Boolean = false
+ ) {
+ sparkMax.restoreFactoryDefaults()
+ sparkMax.idleMode = if (enableBrakeMode) CANSparkMax.IdleMode.kBrake else CANSparkMax.IdleMode.kCoast
+ sparkMax.inverted = inverted
+ if (currentLimit > 0) sparkMax.setSmartCurrentLimit(currentLimit)
+ if (enableVoltageComp) sparkMax.enableVoltageCompensation(RobotController.getBatteryVoltage()) else sparkMax.disableVoltageCompensation()
+ if (controlFrameRateMillis >= 1) sparkMax.setControlFramePeriodMs(controlFrameRateMillis) // Must be between 1 and 100 ms.
+ for ((statusFrame, period) in statusFrameRatesMillis) {
+ sparkMax.setPeriodicFramePeriod(statusFrame, period)
+ }
+
+ for ((slavePort, slaveInverted) in slaveSparks) {
+ val slave = CANSparkMax(slavePort, CANSparkMaxLowLevel.MotorType.kBrushless)
+ slave.restoreFactoryDefaults()
+ slave.follow(sparkMax, slaveInverted)
+ slave.idleMode = sparkMax.idleMode
+ if (currentLimit > 0) slave.setSmartCurrentLimit(currentLimit)
+ slave.burnFlash()
+ }
+
+ when (encoder) {
+ is SparkMaxAbsoluteEncoder -> {
+ encoder.positionConversionFactor = unitPerRotation * gearing
+ encoder.velocityConversionFactor = unitPerRotation * gearing / 60
+ encoder.zeroOffset = offset
+ encoder.setInverted(encInverted)
+ }
+
+ is SparkMaxRelativeEncoder -> {
+ encoder.positionConversionFactor = unitPerRotation * gearing
+ encoder.velocityConversionFactor = unitPerRotation * gearing / 60
+ }
+
+ is SparkMaxAlternateEncoder -> {
+ encoder.positionConversionFactor = unitPerRotation * gearing
+ encoder.velocityConversionFactor = unitPerRotation * gearing / 60
+ encoder.setInverted(encInverted)
+ }
+
+ else -> throw IllegalStateException("UNSUPPORTED ENCODER PLUGGED INTO SPARK MAX.")
+ }
+
+ sparkMax.pidController.setFeedbackDevice(encoder)
+
+ sparkMax.burnFlash()
+ }
+
+ fun enableContinuousInput(sparkMax: CANSparkMax, min: Double, max: Double) {
+ val controller = sparkMax.pidController
+
+ controller.setPositionPIDWrappingEnabled(true)
+ controller.setPositionPIDWrappingMinInput(min)
+ controller.setPositionPIDWrappingMaxInput(max)
+ }
+}
diff --git a/src/main/kotlin/frc/team449/system/encoder/AbsoluteEncoder.kt b/src/main/kotlin/frc/team449/system/encoder/AbsoluteEncoder.kt
new file mode 100644
index 0000000..4016e90
--- /dev/null
+++ b/src/main/kotlin/frc/team449/system/encoder/AbsoluteEncoder.kt
@@ -0,0 +1,105 @@
+package frc.team449.system.encoder
+
+import edu.wpi.first.math.MathUtil
+import edu.wpi.first.math.filter.MedianFilter
+import edu.wpi.first.wpilibj.DutyCycleEncoder
+import edu.wpi.first.wpilibj.Timer
+import edu.wpi.first.wpilibj.motorcontrol.MotorController
+
+/**
+ * This class uses an absolute encoder, gear ratio and UPR to give the absolute position of the module or rotational velocity of the module.
+ *
+ * @param offset This must be in rotations of how much the offset of the ENCODER should be.
+ */
+open class AbsoluteEncoder(
+ name: String,
+ private val enc: DutyCycleEncoder,
+ unitPerRotation: Double,
+ private val inverted: Boolean,
+ private var offset: Double,
+ pollTime: Double = .02,
+ samplesPerAverage: Int = 1
+) : Encoder(name, 1, unitPerRotation, 1.0, pollTime) {
+ private var prevPos = Double.NaN
+ private var prevTime = Double.NaN
+ private val filter = MedianFilter(samplesPerAverage)
+
+ private val frequency = enc.frequency
+
+ /** This returns the absolute position of the module */
+ override fun getPositionNative(): Double {
+ return if (inverted) {
+ filter.calculate(
+ MathUtil.inputModulus(
+ 1 - (enc.absolutePosition - offset),
+ -0.5,
+ 0.5
+ )
+ )
+ } else {
+ filter.calculate(
+ MathUtil.inputModulus(
+ (enc.absolutePosition - offset),
+ -0.5,
+ 0.5
+ )
+
+ )
+ }
+ }
+
+ override fun resetPosition(pos: Double) {
+ offset += getPositionNative() - pos
+ }
+
+ /** This returns the rotational velocity (on vertical axis) of the module */
+ override fun pollVelocityNative(): Double {
+ val currPos =
+ if (inverted) {
+ -enc.distance
+ } else {
+ enc.distance
+ }
+
+ val currTime = Timer.getFPGATimestamp()
+
+ val vel =
+ if (prevPos.isNaN()) {
+ 0.0
+ } else {
+ val dt = currTime - prevTime
+ val dx = currPos - prevPos
+ dx / dt
+ }
+ this.prevPos = currPos
+ this.prevTime = currTime
+
+ return vel
+ }
+
+ companion object {
+ /**
+ * @param
+ * @param channel The DutyCycleEncoder port
+ * @param offset The position to put into DutyCycleEncoder's setPositionOffset
+ * @param unitPerRotation units measured when done one rotation (e.g 360 degrees per rotation)
+ * @param inverted If the encoder needs to be inverted or not
+ */
+ fun creator(
+ channel: Int,
+ offset: Double,
+ unitPerRotation: Double,
+ inverted: Boolean
+ ): EncoderCreator =
+ EncoderCreator { name, _, _ ->
+ val enc = AbsoluteEncoder(
+ name,
+ DutyCycleEncoder(channel),
+ unitPerRotation,
+ inverted,
+ offset
+ )
+ enc
+ }
+ }
+}
diff --git a/src/main/kotlin/frc/team449/system/encoder/BackupEncoder.kt b/src/main/kotlin/frc/team449/system/encoder/BackupEncoder.kt
new file mode 100644
index 0000000..bd1d2e7
--- /dev/null
+++ b/src/main/kotlin/frc/team449/system/encoder/BackupEncoder.kt
@@ -0,0 +1,60 @@
+package frc.team449.system.encoder
+
+import edu.wpi.first.wpilibj.motorcontrol.MotorController
+import kotlin.math.abs
+
+/**
+ * A wrapper to use when you have one external encoder that's more accurate but may be unplugged and
+ * an integrated encoder that's less accurate but is less likely to be unplugged.
+ *
+ * If the primary encoder's velocity is 0 but the integrated encoder's is above a given
+ * threshold, it concludes that the primary encoder is broken and switches to using the
+ * fallback/integrated encoder.
+ */
+class BackupEncoder(
+ private val primary: Encoder,
+ private val fallback: Encoder,
+ private val velThreshold: Double,
+ pollTime: Double = .02
+) : Encoder(primary.name, 1, 1.0, 1.0, pollTime) {
+
+ /** Whether the primary encoder's stopped working */
+ private var useFallback = false
+
+ override fun getPositionNative(): Double {
+ return if (useFallback) {
+ fallback.position
+ } else {
+ primary.position
+ }
+ }
+
+ override fun pollVelocityNative(): Double {
+ val fallbackVel = fallback.velocity
+ return if (useFallback) {
+ fallbackVel
+ } else {
+ val primaryVel = primary.velocity
+ if (primaryVel == 0.0 && abs(fallbackVel) > velThreshold) {
+ this.useFallback = true
+ fallbackVel
+ } else {
+ primaryVel
+ }
+ }
+ }
+
+ companion object {
+ fun creator(
+ primaryCreator: EncoderCreator,
+ fallbackCreator: EncoderCreator,
+ velThreshold: Double
+ ): EncoderCreator = EncoderCreator { name, motor, inverted ->
+ BackupEncoder(
+ primaryCreator.create("primary_$name", motor, inverted),
+ fallbackCreator.create("fallback_$name", motor, inverted),
+ velThreshold
+ )
+ }
+ }
+}
diff --git a/src/main/kotlin/frc/team449/system/encoder/Encoder.kt b/src/main/kotlin/frc/team449/system/encoder/Encoder.kt
new file mode 100644
index 0000000..25a95da
--- /dev/null
+++ b/src/main/kotlin/frc/team449/system/encoder/Encoder.kt
@@ -0,0 +1,116 @@
+package frc.team449.system.encoder
+
+import edu.wpi.first.wpilibj.Timer
+
+/**
+ * A wrapper around encoders. Allows resetting encoders to a position.
+ *
+ * Don't instantiate its subclasses directly. Instead, use their static creator methods
+ * @param encoderCPR Counts per rotation of the encoder
+ * @param unitPerRotation Meters traveled per rotation of the motor
+ * @param gearing The factor the output changes by after being measured by the encoder
+ * (should be >= 1, not a reciprocal), e.g. this would be 70 if there were
+ * a 70:1 gearing between the encoder and the final output
+ */
+abstract class Encoder(
+ val name: String,
+ encoderCPR: Int,
+ unitPerRotation: Double,
+ gearing: Double,
+ private val pollTime: Double
+) {
+ /**
+ * Factor to multiply by to turn native encoder units into meters or whatever units are actually
+ * wanted
+ */
+ private val encoderToUnit = unitPerRotation * gearing / encoderCPR
+
+ /** An offset added to the position to allow resetting position. */
+ private var positionOffset = 0.0
+
+ /** Whether this encoder is being simulated */
+ private var simulated = false
+
+ /** Simulated position set by SimEncoderController */
+ private var simPos = 0.0
+
+ /** Simulated position set by SimEncoderController */
+ private var simVel = 0.0
+
+ /** Used to cache the velocity, because we want to update only when dt >= 20ms **/
+ private var cachedVel = Double.NaN
+ private var cachedTime = Double.NaN
+
+ /** Current position in encoder's units */
+ protected abstract fun getPositionNative(): Double
+
+ /** Current velocity in encoder's units */
+ protected abstract fun pollVelocityNative(): Double
+
+ /** Position in meters or whatever unit you set */
+ val position: Double
+ get() {
+ val posUnits = if (simulated) simPos else this.getPositionDirect()
+ return positionOffset + posUnits
+ }
+
+ /** Velocity in meters per second or whatever unit you set */
+ val velocity: Double
+ get() {
+ return if (simulated) simVel else getVelocityNative() * encoderToUnit
+ }
+
+ /**
+ * Update the position offset to treat the current position as [pos]
+ */
+ open fun resetPosition(pos: Double) {
+ this.positionOffset = pos - this.getPositionDirect()
+ }
+
+ /**
+ * Get the position in units without adding [positionOffset]
+ */
+ private fun getPositionDirect() = this.getPositionNative() * encoderToUnit
+
+ /**
+ * Get the native velocity in units every loopTime
+ */
+ private fun getVelocityNative(): Double {
+ val currTime = Timer.getFPGATimestamp()
+ // update if it has been at least 20 ms since the last update
+ if (cachedTime.isNaN() || currTime - cachedTime >= pollTime) {
+ cachedVel = this.pollVelocityNative()
+ cachedTime = currTime
+ }
+ return cachedVel
+ }
+
+ /**
+ * Used to control [Encoder]s. Only one [SimController] can be used per encoder
+ * object.
+ *
+ * @param enc The encoder to control.
+ */
+ class SimController(private val enc: Encoder) {
+ init {
+ if (enc.simulated) {
+ throw IllegalStateException("${enc.name} is already being simulated.")
+ }
+ enc.simulated = true
+ }
+
+ /** Set the position of the [Encoder] this is controlling. */
+ var position: Double
+ get() = enc.simPos
+ set(pos) {
+ enc.simPos = pos
+ }
+
+ /** Set the velocity of the [Encoder] this is controlling. */
+ var velocity: Double
+ get() = enc.simVel
+ set(vel) {
+ enc.simVel = vel
+ }
+ }
+}
diff --git a/src/main/kotlin/frc/team449/system/encoder/EncoderCreator.kt b/src/main/kotlin/frc/team449/system/encoder/EncoderCreator.kt
new file mode 100644
index 0000000..95ec928
--- /dev/null
+++ b/src/main/kotlin/frc/team449/system/encoder/EncoderCreator.kt
@@ -0,0 +1,12 @@
+package frc.team449.system.encoder
+
+import edu.wpi.first.wpilibj.motorcontrol.MotorController
+
+/**
+ * Create an encoder given a motor controller and its configuration
+ *
+ * @param The type of the motor controller
+ */
+fun interface EncoderCreator {
+ fun create(encName: String, motor: M, inverted: Boolean): Encoder
+}
diff --git a/src/main/kotlin/frc/team449/system/encoder/NEOEncoder.kt b/src/main/kotlin/frc/team449/system/encoder/NEOEncoder.kt
new file mode 100644
index 0000000..69e9579
--- /dev/null
+++ b/src/main/kotlin/frc/team449/system/encoder/NEOEncoder.kt
@@ -0,0 +1,33 @@
+package frc.team449.system.encoder
+
+import com.revrobotics.CANSparkMax
+import com.revrobotics.RelativeEncoder
+
+/** A NEO integrated encoder plugged into a Spark */
+class NEOEncoder(
+ name: String,
+ private val enc: RelativeEncoder,
+ unitPerRotation: Double,
+ gearing: Double,
+ pollTime: Double = .02
+) : Encoder(name, NEO_ENCODER_CPR, 1.0, 1.0, pollTime) {
+
+ init {
+ // Let the underlying encoder do the conversions
+ enc.positionConversionFactor = unitPerRotation * gearing
+ // Divide by 60 because it's originally in RPM
+ enc.velocityConversionFactor = unitPerRotation * gearing / 60
+ }
+
+ override fun getPositionNative() = enc.position
+
+ override fun pollVelocityNative(): Double = enc.velocity
+
+ companion object {
+ const val NEO_ENCODER_CPR = 1
+
+ fun creator(unitPerRotation: Double, gearing: Double): EncoderCreator = EncoderCreator { name, motor, _ ->
+ NEOEncoder(name, motor.encoder, unitPerRotation, gearing)
+ }
+ }
+}
diff --git a/src/main/kotlin/frc/team449/system/encoder/QuadEncoder.kt b/src/main/kotlin/frc/team449/system/encoder/QuadEncoder.kt
new file mode 100644
index 0000000..5b53e82
--- /dev/null
+++ b/src/main/kotlin/frc/team449/system/encoder/QuadEncoder.kt
@@ -0,0 +1,37 @@
+package frc.team449.system.encoder
+
+import edu.wpi.first.wpilibj.motorcontrol.MotorController
+
+/** An external quadrature encoder */
+class QuadEncoder(
+ name: String,
+ private val encoder: edu.wpi.first.wpilibj.Encoder,
+ encoderCPR: Int,
+ unitPerRotation: Double,
+ gearing: Double,
+ pollTime: Double = .02
+) : Encoder(name, 1, 1.0, 1.0, pollTime) {
+ init {
+ // Let the WPI encoder handle the distance scaling
+ encoder.distancePerPulse = unitPerRotation * gearing / encoderCPR
+ encoder.samplesToAverage = 5
+ }
+
+ override fun getPositionNative() = encoder.distance
+
+ override fun pollVelocityNative() = encoder.rate
+
+ companion object {
+ fun creator(
+ encoder: edu.wpi.first.wpilibj.Encoder,
+ encoderCPR: Int,
+ unitPerRotation: Double,
+ gearing: Double,
+ inverted: Boolean
+ ): EncoderCreator =
+ EncoderCreator { name, _, _ ->
+ encoder.setReverseDirection(inverted)
+ QuadEncoder(name, encoder, encoderCPR, unitPerRotation, gearing)
+ }
+ }
+}
diff --git a/src/main/kotlin/frc/team449/system/encoder/TalonEncoder.kt b/src/main/kotlin/frc/team449/system/encoder/TalonEncoder.kt
new file mode 100644
index 0000000..0e697d7
--- /dev/null
+++ b/src/main/kotlin/frc/team449/system/encoder/TalonEncoder.kt
@@ -0,0 +1,29 @@
+package frc.team449.system.encoder
+//
+// import com.ctre.phoenix.motorcontrol.can.BaseTalon
+// import edu.wpi.first.wpilibj.motorcontrol.MotorController
+//
+// /** An encoder plugged into a TalonSRX */
+// class TalonEncoder(
+// name: String,
+// private val talon: BaseTalon,
+// encoderCPR: Int,
+// unitPerRotation: Double,
+// gearing: Double,
+// pollTime: Double = .02
+// ) : Encoder(name, encoderCPR * 4, unitPerRotation, gearing, pollTime) {
+//
+// override fun getPositionNative() = talon.getSelectedSensorPosition(0)
+//
+// override fun pollVelocityNative() = talon.getSelectedSensorVelocity(0)
+//
+// companion object {
+// fun creator(
+// encoderCPR: Int,
+// unitPerRotation: Double,
+// gearing: Double
+// ): EncoderCreator where T : MotorController, T : BaseTalon = EncoderCreator { name, motor, _ ->
+// TalonEncoder(name, motor, encoderCPR, unitPerRotation, gearing)
+// }
+// }
+// }
diff --git a/src/main/kotlin/frc/team449/system/motor/SparkMaxCreator.kt b/src/main/kotlin/frc/team449/system/motor/SparkMaxCreator.kt
new file mode 100644
index 0000000..6cf7614
--- /dev/null
+++ b/src/main/kotlin/frc/team449/system/motor/SparkMaxCreator.kt
@@ -0,0 +1,127 @@
+package frc.team449.system.motor
+
+import com.revrobotics.CANSparkMax
+import com.revrobotics.CANSparkMaxLowLevel
+import com.revrobotics.REVLibError
+import com.revrobotics.SparkMaxLimitSwitch
+import edu.wpi.first.wpilibj.RobotController
+import frc.team449.system.encoder.EncoderCreator
+
+/**
+ * Create a Spark Max with the given configurations
+ *
+ * @param name The motor's name
+ * @param id The motor's CAN ID
+ */
+
+// TODO: Test if disabling voltage compensation helps reduce brownouts
+fun createSparkMax(
+ name: String,
+ id: Int,
+ encCreator: EncoderCreator,
+ enableBrakeMode: Boolean = true,
+ inverted: Boolean = false,
+ currentLimit: Int = 0,
+ enableVoltageComp: Boolean = false,
+ slaveSparks: Map = mapOf(),
+ controlFrameRateMillis: Int = -1,
+ statusFrameRatesMillis: Map = mapOf()
+): WrappedMotor {
+ val motor = CANSparkMax(
+ id,
+ CANSparkMaxLowLevel.MotorType.kBrushless
+ )
+ if (motor.lastError != REVLibError.kOk) {
+ println(
+ "Motor could not be constructed on port " +
+ id +
+ " due to error " +
+ motor.lastError
+ )
+ }
+
+ motor.restoreFactoryDefaults()
+
+ val enc = encCreator.create(name + "Enc", motor, inverted)
+
+ val brakeMode =
+ if (enableBrakeMode) {
+ CANSparkMax.IdleMode.kBrake
+ } else {
+ CANSparkMax.IdleMode.kCoast
+ }
+
+ motor.inverted = inverted
+ // Set brake mode
+ motor.idleMode = brakeMode
+
+ // Set frame rates
+ if (controlFrameRateMillis >= 1) {
+ // Must be between 1 and 100 ms.
+ motor.setControlFramePeriodMs(controlFrameRateMillis)
+ }
+
+ for ((statusFrame, period) in statusFrameRatesMillis) {
+ motor.setPeriodicFramePeriod(statusFrame, period)
+ }
+
+ // Set the current limit if it was given
+ if (currentLimit > 0) {
+ motor.setSmartCurrentLimit(currentLimit)
+ }
+
+ if (enableVoltageComp) {
+ motor.enableVoltageCompensation(RobotController.getBatteryVoltage())
+ } else {
+ motor.disableVoltageCompensation()
+ }
+
+ for ((slavePort, slaveInverted) in slaveSparks) {
+ val slave = createFollowerSpark(slavePort)
+ slave.restoreFactoryDefaults()
+ slave.follow(motor, slaveInverted)
+ slave.idleMode = brakeMode
+ if (currentLimit > 0) {
+ slave.setSmartCurrentLimit(currentLimit)
+ }
+ slave.burnFlash()
+ }
+
+ motor.burnFlash()
+
+ return WrappedMotor(motor, enc)
+}
+
+/**
+ * Create a Spark that will follow another Spark
+ *
+ * @param port The follower's CAN ID
+ */
+private fun createFollowerSpark(port: Int): CANSparkMax {
+ val follower = CANSparkMax(
+ port,
+ CANSparkMaxLowLevel.MotorType.kBrushless
+ )
+
+ follower
+ .getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen)
+ .enableLimitSwitch(false)
+ follower
+ .getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen)
+ .enableLimitSwitch(false)
+
+ follower.setPeriodicFramePeriod(
+ CANSparkMaxLowLevel.PeriodicFrame.kStatus0,
+ 100
+ )
+ follower.setPeriodicFramePeriod(
+ CANSparkMaxLowLevel.PeriodicFrame.kStatus1,
+ 100
+ )
+ follower.setPeriodicFramePeriod(
+ CANSparkMaxLowLevel.PeriodicFrame.kStatus2,
+ 100
+ )
+
+ return follower
+}
diff --git a/src/main/kotlin/frc/team449/system/motor/TalonCreator.kt b/src/main/kotlin/frc/team449/system/motor/TalonCreator.kt
new file mode 100644
index 0000000..4a58f9a
--- /dev/null
+++ b/src/main/kotlin/frc/team449/system/motor/TalonCreator.kt
@@ -0,0 +1,254 @@
+package frc.team449.system.motor
+//
+// import com.ctre.phoenix.motorcontrol.*
+// import com.ctre.phoenix.motorcontrol.can.BaseTalon
+// import com.ctre.phoenix.motorcontrol.can.VictorSPX
+// import com.ctre.phoenix.sensors.SensorVelocityMeasPeriod
+// import edu.wpi.first.wpilibj.motorcontrol.MotorController
+// import frc.team449.system.encoder.EncoderCreator
+//
+// /** Wrap a TalonSRX or TalonFX in a WrappedMotor object
+// * @see configureSlaveTalon
+// * @see createSlaveVictor
+// */
+// fun createTalon(
+// name: String,
+// motor: T,
+// encCreator: EncoderCreator,
+// enableBrakeMode: Boolean = true,
+// inverted: Boolean = false,
+// currentLimit: Int = 0,
+// enableVoltageComp: Boolean = true,
+// voltageCompSamples: Int = 32,
+// slaveTalons: List = listOf(),
+// slaveVictors: List = listOf(),
+// reverseSensor: Boolean = false,
+// controlFrameRatesMillis: Map = mapOf(),
+// statusFrameRatesMillis: Map = mapOf(),
+// feedbackDevice: FeedbackDevice? = null
+// ): WrappedMotor where T : MotorController, T : BaseTalon {
+// val enc = encCreator.create(name + "Enc", motor, inverted)
+// motor.setInverted(inverted)
+// // Set brake mode
+// val idleMode = if (enableBrakeMode) NeutralMode.Brake else NeutralMode.Coast
+// motor.setNeutralMode(idleMode)
+// for ((frame, period) in controlFrameRatesMillis) {
+// motor.setControlFramePeriod(frame, period)
+// }
+// for ((frame, period) in statusFrameRatesMillis) {
+// motor.setStatusFramePeriod(frame, period)
+// }
+//
+// // Setup feedback device if it exists
+// if (feedbackDevice != null) {
+// // CTRE encoder use RPM instead of native units, and can be used as
+// // QuadEncoders, so we switch
+// // them to avoid having to support RPM.
+// if (feedbackDevice == FeedbackDevice.CTRE_MagEncoder_Absolute ||
+// feedbackDevice == FeedbackDevice.CTRE_MagEncoder_Relative
+// ) {
+// motor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, 0)
+// } else {
+// motor.configSelectedFeedbackSensor(feedbackDevice, 0, 0)
+// }
+// motor.setSensorPhase(reverseSensor)
+// } else {
+// motor.configSelectedFeedbackSensor(FeedbackDevice.None, 0, 0)
+// }
+//
+// // Set the current limit if it was given
+// if (currentLimit > 0) {
+// motor.configSupplyCurrentLimit(
+// SupplyCurrentLimitConfiguration(true, currentLimit.toDouble(), 0.0, 0.0),
+// 0
+// )
+// } else {
+// // If we don't have a current limit, disable current limiting.
+// motor.configSupplyCurrentLimit(SupplyCurrentLimitConfiguration(), 0)
+// }
+//
+// // Enable or disable voltage comp
+// if (enableVoltageComp) {
+// motor.enableVoltageCompensation(true)
+// motor.configVoltageCompSaturation(12.0, 0)
+// }
+// motor.configVoltageMeasurementFilter(voltageCompSamples, 0)
+//
+// // Use slot 0
+// motor.selectProfileSlot(0, 0)
+// val port = motor.deviceID
+// // Set up slaves.
+// for (slave in slaveTalons) {
+// setMasterForTalon(
+// slave,
+// port,
+// enableBrakeMode,
+// currentLimit,
+// if (enableVoltageComp) voltageCompSamples else null
+// )
+// }
+// for (slave in slaveVictors) {
+// setMasterForVictor(
+// slave,
+// motor,
+// enableBrakeMode,
+// if (enableVoltageComp) voltageCompSamples else null
+// )
+// }
+// motor.configVelocityMeasurementPeriod(SensorVelocityMeasPeriod.Period_10Ms)
+// motor.configVelocityMeasurementWindow(10)
+//
+// // Set max voltage
+// motor.configPeakOutputForward(1.0, 0)
+// motor.configPeakOutputReverse(1.0, 0)
+//
+// motor.configNominalOutputForward(0.0, 0)
+// motor.configNominalOutputReverse(0.0, 0)
+//
+// return WrappedMotor(name, motor, enc)
+// }
+//
+// /** @param slaveTalon Takes a slave TalonSRX or TalonFX and configures it to act as a slave
+// * @return The same Talon
+// */
+// fun configureSlaveTalon(
+// slaveTalon: BaseTalon,
+// invertType: InvertType
+// ): BaseTalon {
+// // Turn off features we don't want a slave to have
+// slaveTalon.setInverted(invertType)
+// slaveTalon.configForwardLimitSwitchSource(
+// LimitSwitchSource.Deactivated,
+// LimitSwitchNormal.Disabled,
+// 0
+// )
+// slaveTalon.configReverseLimitSwitchSource(
+// LimitSwitchSource.Deactivated,
+// LimitSwitchNormal.Disabled,
+// 0
+// )
+// slaveTalon.configForwardSoftLimitEnable(false, 0)
+// slaveTalon.configReverseSoftLimitEnable(false, 0)
+// slaveTalon.configPeakOutputForward(1.0, 0)
+// slaveTalon.enableVoltageCompensation(true)
+// slaveTalon.configVoltageCompSaturation(12.0, 0)
+// slaveTalon.configVoltageMeasurementFilter(32, 0)
+//
+// // Slow down frames so we don't overload the CAN bus
+// slaveTalon.setStatusFramePeriod(StatusFrameEnhanced.Status_1_General, 100, 0)
+// slaveTalon.setStatusFramePeriod(StatusFrameEnhanced.Status_6_Misc, 100, 0)
+// slaveTalon.setStatusFramePeriod(StatusFrameEnhanced.Status_7_CommStatus, 100, 0)
+// slaveTalon.setStatusFramePeriod(StatusFrameEnhanced.Status_9_MotProfBuffer, 100, 0)
+// slaveTalon.setStatusFramePeriod(StatusFrameEnhanced.Status_10_MotionMagic, 100, 0)
+// slaveTalon.setStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, 100, 0)
+// slaveTalon.setStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, 100, 0)
+// slaveTalon.setStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, 100, 0)
+// slaveTalon.setStatusFramePeriod(StatusFrameEnhanced.Status_15_FirmwareApiStatus, 100, 0)
+// slaveTalon.setStatusFramePeriod(StatusFrameEnhanced.Status_3_Quadrature, 100, 0)
+// slaveTalon.setStatusFramePeriod(StatusFrameEnhanced.Status_8_PulseWidth, 100, 0)
+// slaveTalon.setStatusFramePeriod(StatusFrameEnhanced.Status_11_UartGadgeteer, 100, 0)
+//
+// return slaveTalon
+// }
+//
+// /**
+// * Creates a VictorSPX that will follow some Talon
+// *
+// * @param port The CAN ID of this Victor SPX.
+// * @param invertType Whether to invert this relative to the master. Defaults to not inverting
+// * relative to master.
+// */
+// fun createSlaveVictor(port: Int, invertType: InvertType): VictorSPX {
+// val victorSPX = VictorSPX(port)
+// victorSPX.setInverted(invertType)
+// victorSPX.configPeakOutputForward(1.0, 0)
+// victorSPX.configPeakOutputReverse(-1.0, 0)
+// victorSPX.enableVoltageCompensation(true)
+// victorSPX.configVoltageCompSaturation(12.0, 0)
+// victorSPX.configVoltageMeasurementFilter(32, 0)
+// victorSPX.setStatusFramePeriod(StatusFrame.Status_1_General, 100, 0)
+// victorSPX.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 100, 0)
+// victorSPX.setStatusFramePeriod(StatusFrame.Status_6_Misc, 100, 0)
+// victorSPX.setStatusFramePeriod(StatusFrame.Status_7_CommStatus, 100, 0)
+// victorSPX.setStatusFramePeriod(StatusFrame.Status_9_MotProfBuffer, 100, 0)
+// victorSPX.setStatusFramePeriod(StatusFrame.Status_10_MotionMagic, 100, 0)
+// victorSPX.setStatusFramePeriod(StatusFrame.Status_12_Feedback1, 100, 0)
+// victorSPX.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 100, 0)
+// victorSPX.setStatusFramePeriod(StatusFrame.Status_14_Turn_PIDF1, 100, 0)
+//
+// return victorSPX
+// }
+//
+// /**
+// * Set this Talon to follow another CAN device.
+// *
+// * @param port The CAN ID of the device to follow.
+// * @param brakeMode Whether this Talon should be in brake mode or coast mode.
+// * @param currentLimit The current limit for this Talon. Can be null for no current limit.
+// * @param voltageCompSamples The number of voltage compensation samples to use, or null to not
+// * compensate voltage.
+// */
+// private fun setMasterForTalon(
+// slaveTalon: BaseTalon,
+// port: Int,
+// brakeMode: Boolean,
+// currentLimit: Int,
+// voltageCompSamples: Int?
+// ) {
+// // Brake mode doesn't automatically follow master
+// slaveTalon.setNeutralMode(if (brakeMode) NeutralMode.Brake else NeutralMode.Coast)
+//
+// // Current limiting might not automatically follow master, set it just to be
+// // safe
+// if (currentLimit > 0) {
+// slaveTalon.configSupplyCurrentLimit(
+// SupplyCurrentLimitConfiguration(true, currentLimit.toDouble(), 0.0, 0.0),
+// 0
+// )
+// } else {
+// // If we don't have a current limit, disable current limiting.
+// slaveTalon.configSupplyCurrentLimit(SupplyCurrentLimitConfiguration(), 0)
+// }
+//
+// // Voltage comp might not follow master either
+// if (voltageCompSamples != null) {
+// slaveTalon.enableVoltageCompensation(true)
+// slaveTalon.configVoltageCompSaturation(12.0, 0)
+// slaveTalon.configVoltageMeasurementFilter(voltageCompSamples, 0)
+// } else {
+// slaveTalon.enableVoltageCompensation(false)
+// }
+//
+// // Follow the leader
+// slaveTalon[ControlMode.Follower] = port.toDouble()
+// }
+//
+// /**
+// * Set this Victor to follow another CAN device.
+// *
+// * @param toFollow The motor controller to follow.
+// * @param brakeMode Whether this Talon should be in brake mode or coast mode.
+// * @param voltageCompSamples The number of voltage compensation samples to use, or null to not
+// * compensate voltage.
+// */
+// private fun setMasterForVictor(
+// victorSPX: VictorSPX,
+// toFollow: IMotorController,
+// brakeMode: Boolean,
+// voltageCompSamples: Int?
+// ) {
+// // Brake mode doesn't automatically follow master
+// victorSPX.setNeutralMode(if (brakeMode) NeutralMode.Brake else NeutralMode.Coast)
+//
+// // Voltage comp might not follow master either
+// if (voltageCompSamples != null) {
+// victorSPX.enableVoltageCompensation(true)
+// victorSPX.configVoltageCompSaturation(12.0, 0)
+// victorSPX.configVoltageMeasurementFilter(voltageCompSamples, 0)
+// } else {
+// victorSPX.enableVoltageCompensation(false)
+// }
+//
+// // Follow the leader
+// victorSPX.follow(toFollow)
+// }
diff --git a/src/main/kotlin/frc/team449/system/motor/WrappedMotor.kt b/src/main/kotlin/frc/team449/system/motor/WrappedMotor.kt
new file mode 100644
index 0000000..8301988
--- /dev/null
+++ b/src/main/kotlin/frc/team449/system/motor/WrappedMotor.kt
@@ -0,0 +1,35 @@
+package frc.team449.system.motor
+
+import edu.wpi.first.wpilibj.RobotController
+import edu.wpi.first.wpilibj.motorcontrol.MotorController
+import frc.team449.system.encoder.Encoder
+
+/** Our own wrapper grouping the motor controller and encoder for a motor */
+class WrappedMotor(
+ private val motor: MotorController,
+ val encoder: Encoder
+) : MotorController by motor {
+ /**
+ * The last set voltage for this motor (through [setVoltage] or [set])
+ */
+ var lastVoltage = 0.0
+ private set
+
+ /** Position in meters or whatever unit you set */
+ val position: Double
+ get() = encoder.position
+
+ /** Velocity in meters per second or whatever unit you set */
+ val velocity: Double
+ get() = encoder.velocity
+
+ override fun setVoltage(volts: Double) {
+ motor.setVoltage(volts)
+ this.lastVoltage = volts
+ }
+
+ override fun set(output: Double) {
+ motor.set(output)
+ this.lastVoltage = output * RobotController.getBatteryVoltage()
+ }
+}
diff --git a/src/main/kotlin/frc/team449/util/Properties.kt b/src/main/kotlin/frc/team449/util/Properties.kt
new file mode 100644
index 0000000..577a5c2
--- /dev/null
+++ b/src/main/kotlin/frc/team449/util/Properties.kt
@@ -0,0 +1,37 @@
+package frc.team449.util
+
+import edu.wpi.first.hal.SimBoolean
+import edu.wpi.first.hal.SimDouble
+import kotlin.properties.ReadOnlyProperty
+import kotlin.properties.ReadWriteProperty
+import kotlin.reflect.KProperty
+
+/** A property with a custom getter but a backing field of a different type
+ * @tparam B The type of the actual backing field
+ * @tparam F The type of the property that is delegated
+ */
+fun wrappedProp(backing: B, get: (B) -> F): ReadOnlyProperty {
+ return object : ReadOnlyProperty {
+ override operator fun getValue(thisRef: Any?, property: KProperty<*>) = get(backing)
+ }
+}
+
+/** A property with a custom getter and setter but a backing field of a different type
+ * @tparam B The type of the actual backing field
+ * @tparam F The type of the property that is delegated
+ * @see simDoubleProp
+ * @see simBooleanProp
+ */
+fun wrappedProp(backing: B, get: (B) -> F, set: (B, F) -> Unit): ReadWriteProperty {
+ return object : ReadWriteProperty {
+ override operator fun getValue(thisRef: Any?, property: KProperty<*>) = get(backing)
+
+ override operator fun setValue(thisRef: Any?, property: KProperty<*>, value: F) = set(backing, value)
+ }
+}
+
+/** A delegated property of type Double backed by a [SimDouble] */
+fun simDoubleProp(sim: SimDouble): ReadWriteProperty = wrappedProp(sim, SimDouble::get, SimDouble::set)
+
+/** A delegated property of type Boolean backed by a [SimBoolean] */
+fun simBooleanProp(sim: SimBoolean): ReadWriteProperty = wrappedProp(sim, SimBoolean::get, SimBoolean::set)
diff --git a/src/main/resources/2023-field.png b/src/main/resources/2023-field.png
new file mode 100644
index 0000000..51cc6a3
Binary files /dev/null and b/src/main/resources/2023-field.png differ
diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json
new file mode 100644
index 0000000..2a29835
--- /dev/null
+++ b/vendordeps/NavX.json
@@ -0,0 +1,40 @@
+{
+ "fileName": "NavX.json",
+ "name": "NavX",
+ "version": "2024.0.1-beta-4",
+ "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b",
+ "frcYear": "2024",
+ "mavenUrls": [
+ "https://dev.studica.com/maven/release/2024/"
+ ],
+ "jsonUrl": "https://dev.studica.com/releases/2024/NavX.json",
+ "javaDependencies": [
+ {
+ "groupId": "com.kauailabs.navx.frc",
+ "artifactId": "navx-frc-java",
+ "version": "2024.0.1-beta-4"
+ }
+ ],
+ "jniDependencies": [],
+ "cppDependencies": [
+ {
+ "groupId": "com.kauailabs.navx.frc",
+ "artifactId": "navx-frc-cpp",
+ "version": "2024.0.1-beta-4",
+ "headerClassifier": "headers",
+ "sourcesClassifier": "sources",
+ "sharedLibrary": false,
+ "libName": "navx_frc",
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "linuxathena",
+ "linuxraspbian",
+ "linuxarm32",
+ "linuxarm64",
+ "linuxx86-64",
+ "osxuniversal",
+ "windowsx86-64"
+ ]
+ }
+ ]
+}
\ No newline at end of file
diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json
new file mode 100644
index 0000000..31501f6
--- /dev/null
+++ b/vendordeps/PathplannerLib.json
@@ -0,0 +1,38 @@
+{
+ "fileName": "PathplannerLib.json",
+ "name": "PathplannerLib",
+ "version": "2024.0.0-beta-5.1",
+ "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
+ "frcYear": "2024",
+ "mavenUrls": [
+ "https://3015rangerrobotics.github.io/pathplannerlib/repo"
+ ],
+ "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json",
+ "javaDependencies": [
+ {
+ "groupId": "com.pathplanner.lib",
+ "artifactId": "PathplannerLib-java",
+ "version": "2024.0.0-beta-5.1"
+ }
+ ],
+ "jniDependencies": [],
+ "cppDependencies": [
+ {
+ "groupId": "com.pathplanner.lib",
+ "artifactId": "PathplannerLib-cpp",
+ "version": "2024.0.0-beta-5.1",
+ "libName": "PathplannerLib",
+ "headerClassifier": "headers",
+ "sharedLibrary": false,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxx86-64",
+ "osxuniversal",
+ "linuxathena",
+ "linuxarm32",
+ "linuxarm64"
+ ]
+ }
+ ]
+}
diff --git a/vendordeps/PhotonLib-json-1.0.json b/vendordeps/PhotonLib-json-1.0.json
new file mode 100644
index 0000000..748c671
--- /dev/null
+++ b/vendordeps/PhotonLib-json-1.0.json
@@ -0,0 +1,42 @@
+{
+ "fileName": "photonlib.json",
+ "name": "photonlib",
+ "version": "v2024.1.1-beta-3.2",
+ "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
+ "frcYear": "2024",
+ "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": [],
+ "cppDependencies": [
+ {
+ "groupId": "org.photonvision",
+ "artifactId": "PhotonLib-cpp",
+ "version": "v2024.1.1-beta-3.2",
+ "libName": "Photon",
+ "headerClassifier": "headers",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "linuxathena",
+ "linuxx86-64",
+ "osxuniversal"
+ ]
+ }
+ ],
+ "javaDependencies": [
+ {
+ "groupId": "org.photonvision",
+ "artifactId": "PhotonLib-java",
+ "version": "v2024.1.1-beta-3.2"
+ },
+ {
+ "groupId": "org.photonvision",
+ "artifactId": "PhotonTargeting-java",
+ "version": "v2024.1.1-beta-3.2"
+ }
+ ]
+}
diff --git a/vendordeps/REVLib-2024.json b/vendordeps/REVLib-2024.json
new file mode 100644
index 0000000..8a64caf
--- /dev/null
+++ b/vendordeps/REVLib-2024.json
@@ -0,0 +1,74 @@
+{
+ "fileName": "REVLib.json",
+ "name": "REVLib",
+ "version": "2024.0.0",
+ "frcYear": "2024",
+ "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
+ "mavenUrls": [
+ "https://maven.revrobotics.com/"
+ ],
+ "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json",
+ "javaDependencies": [
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "REVLib-java",
+ "version": "2024.0.0"
+ }
+ ],
+ "jniDependencies": [
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "REVLib-driver",
+ "version": "2024.0.0",
+ "skipInvalidPlatforms": true,
+ "isJar": false,
+ "validPlatforms": [
+ "windowsx86-64",
+ "windowsx86",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxathena",
+ "linuxarm32",
+ "osxuniversal"
+ ]
+ }
+ ],
+ "cppDependencies": [
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "REVLib-cpp",
+ "version": "2024.0.0",
+ "libName": "REVLib",
+ "headerClassifier": "headers",
+ "sharedLibrary": false,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "windowsx86",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxathena",
+ "linuxarm32",
+ "osxuniversal"
+ ]
+ },
+ {
+ "groupId": "com.revrobotics.frc",
+ "artifactId": "REVLib-driver",
+ "version": "2024.0.0",
+ "libName": "REVLibDriver",
+ "headerClassifier": "headers",
+ "sharedLibrary": false,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "windowsx86-64",
+ "windowsx86",
+ "linuxarm64",
+ "linuxx86-64",
+ "linuxathena",
+ "linuxarm32",
+ "osxuniversal"
+ ]
+ }
+ ]
+}
diff --git a/vendordeps/WPILibNewCommands.json b/vendordeps/WPILibNewCommands.json
new file mode 100644
index 0000000..67bf389
--- /dev/null
+++ b/vendordeps/WPILibNewCommands.json
@@ -0,0 +1,38 @@
+{
+ "fileName": "WPILibNewCommands.json",
+ "name": "WPILib-New-Commands",
+ "version": "1.0.0",
+ "uuid": "111e20f7-815e-48f8-9dd6-e675ce75b266",
+ "frcYear": "2024",
+ "mavenUrls": [],
+ "jsonUrl": "",
+ "javaDependencies": [
+ {
+ "groupId": "edu.wpi.first.wpilibNewCommands",
+ "artifactId": "wpilibNewCommands-java",
+ "version": "wpilib"
+ }
+ ],
+ "jniDependencies": [],
+ "cppDependencies": [
+ {
+ "groupId": "edu.wpi.first.wpilibNewCommands",
+ "artifactId": "wpilibNewCommands-cpp",
+ "version": "wpilib",
+ "libName": "wpilibNewCommands",
+ "headerClassifier": "headers",
+ "sourcesClassifier": "sources",
+ "sharedLibrary": true,
+ "skipInvalidPlatforms": true,
+ "binaryPlatforms": [
+ "linuxathena",
+ "linuxarm32",
+ "linuxarm64",
+ "windowsx86-64",
+ "windowsx86",
+ "linuxx86-64",
+ "osxuniversal"
+ ]
+ }
+ ]
+}