diff --git a/build.gradle.kts b/build.gradle.kts index a2bd07a..e046f14 100644 --- a/build.gradle.kts +++ b/build.gradle.kts @@ -155,11 +155,6 @@ tasks { from({ sourceSets.main.get().allSource }) } - - register("replayWatch", JavaExec::class.java) { - mainClass = "org.littletonrobotics.junction.ReplayWatch" - classpath = sourceSets.main.get().runtimeClasspath - } } gversion { diff --git a/src/main/kotlin/org/frc5183/robot/Robot.kt b/src/main/kotlin/org/frc5183/robot/Robot.kt index 6c04502..89f5c9f 100644 --- a/src/main/kotlin/org/frc5183/robot/Robot.kt +++ b/src/main/kotlin/org/frc5183/robot/Robot.kt @@ -4,17 +4,12 @@ import edu.wpi.first.hal.FRCNetComm import edu.wpi.first.hal.HAL import edu.wpi.first.math.geometry.Pose2d import edu.wpi.first.units.Units -import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj.Threads import edu.wpi.first.wpilibj.util.WPILibVersion import edu.wpi.first.wpilibj2.command.CommandScheduler -import org.frc5183.robot.commands.drive.TeleopDriveCommand import org.frc5183.robot.constants.* import org.frc5183.robot.constants.swerve.SwerveConstants -import org.frc5183.robot.constants.swerve.SwervePIDConstants import org.frc5183.robot.subsystems.drive.SwerveDriveSubsystem -import org.frc5183.robot.subsystems.drive.io.RealSwerveDriveIO -import org.frc5183.robot.subsystems.drive.io.SimulatedSwerveDriveIO import org.littletonrobotics.junction.LoggedRobot import org.littletonrobotics.junction.Logger import swervelib.SwerveDrive @@ -47,20 +42,16 @@ object Robot : LoggedRobot() { Logger.start() - val swerve = - SwerveDrive( - SwerveConstants.YAGSL, - SwerveConstants.YAGSL_CONTROLLER_CONFIG, - PhysicalConstants.MAX_VELOCITY.`in`(Units.MetersPerSecond), - Pose2d.kZero, - ) - drive = - if (RobotBase.isReal()) { - SwerveDriveSubsystem(RealSwerveDriveIO(swerve)) - } else { - SwerveDriveSubsystem(SimulatedSwerveDriveIO(swerve)) - } + SwerveDriveSubsystem( + SwerveDrive( + SwerveConstants.YAGSL, + SwerveConstants.YAGSL_CONTROLLER_CONFIG, + PhysicalConstants.MAX_VELOCITY.`in`(Units.MetersPerSecond), + Pose2d.kZero, + ), + null, + ) } override fun robotPeriodic() { diff --git a/src/main/kotlin/org/frc5183/robot/constants/Controls.kt b/src/main/kotlin/org/frc5183/robot/constants/Controls.kt index 490b3d7..ecbb5d8 100644 --- a/src/main/kotlin/org/frc5183/robot/constants/Controls.kt +++ b/src/main/kotlin/org/frc5183/robot/constants/Controls.kt @@ -1,29 +1,22 @@ package org.frc5183.robot.constants +import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.CommandScheduler import edu.wpi.first.wpilibj2.command.button.CommandXboxController import org.frc5183.robot.commands.drive.TeleopDriveCommand import org.frc5183.robot.subsystems.drive.SwerveDriveSubsystem +import swervelib.SwerveInputStream import kotlin.math.absoluteValue object Controls { val DRIVER_CONTROLLER: CommandXboxController = CommandXboxController(0) val OPERATOR_CONTROLLER: CommandXboxController = CommandXboxController(1) - lateinit var TELEOP_DRIVE_COMMAND: TeleopDriveCommand + lateinit var DRIVE_COMMAND: Command fun registerControls(drive: SwerveDriveSubsystem) { CommandScheduler.getInstance().activeButtonLoop.clear() - TELEOP_DRIVE_COMMAND = - TeleopDriveCommand( - drive, - xInput = { if (DRIVER_CONTROLLER.leftX.absoluteValue < .2) 0.0 else DRIVER_CONTROLLER.leftX }, - yInput = { if (DRIVER_CONTROLLER.leftY.absoluteValue < .2) 0.0 else DRIVER_CONTROLLER.leftY }, - rotationInput = { 0.0 }, - fieldRelative = false, - ) - drive.defaultCommand = TELEOP_DRIVE_COMMAND } } diff --git a/src/main/kotlin/org/frc5183/robot/logging/AutoLogInputs.kt b/src/main/kotlin/org/frc5183/robot/logging/AutoLogInputs.kt deleted file mode 100644 index 5b1c589..0000000 --- a/src/main/kotlin/org/frc5183/robot/logging/AutoLogInputs.kt +++ /dev/null @@ -1,141 +0,0 @@ -package org.frc5183.robot.logging - -import edu.wpi.first.units.Measure -import edu.wpi.first.units.MutableMeasure -import edu.wpi.first.util.struct.Struct -import edu.wpi.first.util.struct.StructSerializable -import org.littletonrobotics.junction.LogTable -import org.littletonrobotics.junction.inputs.LoggableInputs -import kotlin.reflect.KProperty -import edu.wpi.first.units.Unit as WPIUnit - -/** - * An abstract class that can be implemented as a replacement for the - * @AutoLog AdvantageKit annotation for Kotlin codebases. - * - * @author Daniel1464 https://github.com/Mechanical-Advantage/AdvantageKit/pull/112 - */ -abstract class AutoLogInputs : LoggableInputs { - fun log( - value: Double, - key: String? = null, - ) = LoggedInput(value, key, LogTable::put, LogTable::get) - - fun log( - value: Int, - key: String? = null, - ) = LoggedInput(value, key, LogTable::put, LogTable::get) - - fun log( - value: String, - key: String? = null, - ) = LoggedInput(value, key, LogTable::put, LogTable::get) - - fun log( - value: Boolean, - key: String? = null, - ) = LoggedInput(value, key, LogTable::put, LogTable::get) - - fun log( - value: Long, - key: String? = null, - ) = LoggedInput(value, key, LogTable::put, LogTable::get) - - fun log( - value: T, - key: String? = null, - ) = LoggedInput(value, key, LogTable::put, LogTable::get) - - fun log( - value: T?, - struct: Struct, - key: String? = null, - ) = LoggedInput(value, key, { k, v -> put(k, struct, v) }, { k, v -> get(k, struct, v) }) - - fun log( - value: Measure, - key: String? = null, - ) = LoggedInput(value, key, LogTable::put, LogTable::get) - - fun , M : MutableMeasure> log( - value: M, - key: String? = null, - ) = LoggedInput(value, key, LogTable::put, LogTable::get) - - fun log( - value: DoubleArray, - key: String? = null, - ) = LoggedInput(value, key, LogTable::put, LogTable::get) - - fun log( - value: IntArray, - key: String? = null, - ) = LoggedInput(value, key, LogTable::put, LogTable::get) - - fun log( - value: Array, - key: String? = null, - ) = LoggedInput(value, key, LogTable::put, LogTable::get) - - fun log( - value: BooleanArray, - key: String? = null, - ) = LoggedInput(value, key, LogTable::put, LogTable::get) - - fun log( - value: LongArray, - key: String? = null, - ) = LoggedInput(value, key, LogTable::put, LogTable::get) - - fun log( - value: Array, - key: String? = null, - ) = LoggedInput(value, key, LogTable::put, LogTable::get) - - fun log( - value: Array, - struct: Struct, - key: String? = null, - ) = LoggedInput(value, key, { k, v -> put(k, struct, *v) }, { k, _ -> get(k, struct) }) - - private val toLogRunners = mutableListOf<(LogTable) -> Unit>() - private val fromLogRunners = mutableListOf<(LogTable) -> Unit>() - - inner class LoggedInput( - private var value: T, - private val name: String? = null, - private val toLog: LogTable.(String, T) -> Unit, - private val fromLog: LogTable.(String, T) -> T, - ) { - operator fun getValue( - thisRef: Any, - property: KProperty<*>, - ) = value - - operator fun setValue( - thisRef: Any, - property: KProperty<*>, - value: T, - ) { - this.value = value - } - - operator fun provideDelegate( - thisRef: Any, - property: KProperty<*>, - ): LoggedInput { - val namespace = this.name ?: property.name - toLogRunners.add { logTable -> this.toLog(logTable, namespace, value) } - fromLogRunners.add { logTable -> value = this.fromLog(logTable, namespace, value) } - return this - } - } - - override fun fromLog(table: LogTable) { - fromLogRunners.forEach { it(table) } - } - - override fun toLog(table: LogTable) { - toLogRunners.forEach { it(table) } - } -} diff --git a/src/main/kotlin/org/frc5183/robot/logging/struct/EstimatedRobotPoseStruct.kt b/src/main/kotlin/org/frc5183/robot/logging/struct/EstimatedRobotPoseStruct.kt deleted file mode 100644 index e667efe..0000000 --- a/src/main/kotlin/org/frc5183/robot/logging/struct/EstimatedRobotPoseStruct.kt +++ /dev/null @@ -1,54 +0,0 @@ -package org.frc5183.robot.logging.struct - -import edu.wpi.first.math.geometry.Pose3d -import edu.wpi.first.util.struct.Struct -import org.photonvision.EstimatedRobotPose -import org.photonvision.targeting.PhotonTrackedTarget -import java.nio.ByteBuffer - -// todo: some things here are missing which might mess with simulation. works for basic logging tho -object EstimatedRobotPoseStruct : Struct { - override fun getTypeClass(): Class = EstimatedRobotPose::class.java - - override fun getTypeName(): String = "EstimatedRobotPose" - - override fun getSize(): Int = Struct.kSizeDouble * 7 - - override fun getSchema(): String = "double poseX;double poseY;double poseZ;double rotX;double rotY;double rotZ;double timestampSeconds" - - override fun unpack(bb: ByteBuffer): EstimatedRobotPose { - val poseX = bb.getDouble() - val poseY = bb.getDouble() - val poseZ = bb.getDouble() - val rotX = bb.getDouble() - val rotY = bb.getDouble() - val rotZ = bb.getDouble() - - val pose3d = - Pose3d( - poseX, - poseY, - poseZ, - edu.wpi.first.math.geometry - .Rotation3d(rotX, rotY, rotZ), - ) - - val timestampSeconds = bb.getDouble() - - return EstimatedRobotPose(pose3d, timestampSeconds, emptyList()) - } - - override fun pack( - bb: ByteBuffer, - value: EstimatedRobotPose, - ) { - bb.putDouble(value.estimatedPose.x) - bb.putDouble(value.estimatedPose.y) - bb.putDouble(value.estimatedPose.z) - bb.putDouble(value.estimatedPose.rotation.x) - bb.putDouble(value.estimatedPose.rotation.y) - bb.putDouble(value.estimatedPose.rotation.z) - - bb.putDouble(value.timestampSeconds) - } -} diff --git a/src/main/kotlin/org/frc5183/robot/logging/struct/PhotonTrackedTargetStruct.kt b/src/main/kotlin/org/frc5183/robot/logging/struct/PhotonTrackedTargetStruct.kt deleted file mode 100644 index b8b59bf..0000000 --- a/src/main/kotlin/org/frc5183/robot/logging/struct/PhotonTrackedTargetStruct.kt +++ /dev/null @@ -1,37 +0,0 @@ -package org.frc5183.robot.logging.struct - -import edu.wpi.first.util.struct.Struct -import org.photonvision.targeting.PhotonTrackedTarget -import java.nio.ByteBuffer - -// todo: some things here are missing which might mess with simulation. works for basic logging tho -object PhotonTrackedTargetStruct : Struct { - override fun getTypeClass(): Class = PhotonTrackedTarget::class.java - - override fun getTypeName(): String = "PhotonTrackedTarget" - - override fun getSize(): Int = Struct.kSizeDouble * 4 + Struct.kSizeInt32 - - override fun getSchema(): String = "double yaw;double pitch;double area;double skew;int32 fiducialId;" - - override fun unpack(bb: ByteBuffer): PhotonTrackedTarget { - val target = PhotonTrackedTarget() - target.yaw = bb.getDouble() - target.pitch = bb.getDouble() - target.area = bb.getDouble() - target.skew = bb.getDouble() - target.fiducialId = bb.getInt() - return target - } - - override fun pack( - bb: ByteBuffer, - value: PhotonTrackedTarget, - ) { - bb.putDouble(value.yaw) - bb.putDouble(value.pitch) - bb.putDouble(value.area) - bb.putDouble(value.skew) - bb.putInt(value.fiducialId) - } -} diff --git a/src/main/kotlin/org/frc5183/robot/subsystems/collector/CollectorSubsystem.kt b/src/main/kotlin/org/frc5183/robot/subsystems/collector/CollectorSubsystem.kt index 4b46d1d..db4578f 100644 --- a/src/main/kotlin/org/frc5183/robot/subsystems/collector/CollectorSubsystem.kt +++ b/src/main/kotlin/org/frc5183/robot/subsystems/collector/CollectorSubsystem.kt @@ -1,31 +1,31 @@ package org.frc5183.robot.subsystems.collector +import com.revrobotics.spark.SparkMax import edu.wpi.first.wpilibj2.command.SubsystemBase -import org.frc5183.robot.subsystems.collector.io.CollectorIO -import org.frc5183.robot.subsystems.collector.io.CollectorIOInputs +import org.littletonrobotics.junction.Logger class CollectorSubsystem( - private val io: CollectorIO, + val arm: SparkMax, + val intake: SparkMax, ) : SubsystemBase() { - private val ioInputs = CollectorIOInputs() - override fun periodic() { - io.updateInputs(ioInputs) + Logger.recordOutput("Collector/ArmSpeed", arm.get()) + Logger.recordOutput("Collector/IntakeSpeed", intake.get()) } fun runArm(speed: Double) { - io.runArm(speed) + arm.set(speed) } fun runIntake(speed: Double) { - io.runIntake(speed) + intake.set(speed) } fun stopArm() { - io.stopArm() + arm.stopMotor() } fun stopIntake() { - io.stopIntake() + intake.stopMotor() } } diff --git a/src/main/kotlin/org/frc5183/robot/subsystems/collector/io/CollectorIO.kt b/src/main/kotlin/org/frc5183/robot/subsystems/collector/io/CollectorIO.kt deleted file mode 100644 index b3cd437..0000000 --- a/src/main/kotlin/org/frc5183/robot/subsystems/collector/io/CollectorIO.kt +++ /dev/null @@ -1,13 +0,0 @@ -package org.frc5183.robot.subsystems.collector.io - -interface CollectorIO { - fun updateInputs(inputs: CollectorIOInputs) - - fun runArm(speed: Double) - - fun runIntake(speed: Double) - - fun stopArm() - - fun stopIntake() -} diff --git a/src/main/kotlin/org/frc5183/robot/subsystems/collector/io/CollectorIOInputs.kt b/src/main/kotlin/org/frc5183/robot/subsystems/collector/io/CollectorIOInputs.kt deleted file mode 100644 index a28a1d0..0000000 --- a/src/main/kotlin/org/frc5183/robot/subsystems/collector/io/CollectorIOInputs.kt +++ /dev/null @@ -1,8 +0,0 @@ -package org.frc5183.robot.subsystems.collector.io - -import org.frc5183.robot.logging.AutoLogInputs - -class CollectorIOInputs : AutoLogInputs() { - var collectorArmSpeed by log(0.0) - var collectorIntakeSpeed by log(0.0) -} diff --git a/src/main/kotlin/org/frc5183/robot/subsystems/collector/io/RealCollectorIO.kt b/src/main/kotlin/org/frc5183/robot/subsystems/collector/io/RealCollectorIO.kt deleted file mode 100644 index e97a023..0000000 --- a/src/main/kotlin/org/frc5183/robot/subsystems/collector/io/RealCollectorIO.kt +++ /dev/null @@ -1,29 +0,0 @@ -package org.frc5183.robot.subsystems.collector.io - -import com.revrobotics.spark.SparkMax - -class RealCollectorIO( - val arm: SparkMax, - val intake: SparkMax, -) : CollectorIO { - override fun updateInputs(inputs: CollectorIOInputs) { - inputs.collectorArmSpeed = arm.get() - inputs.collectorIntakeSpeed = intake.get() - } - - override fun runArm(speed: Double) { - arm.set(speed) - } - - override fun runIntake(speed: Double) { - intake.set(speed) - } - - override fun stopArm() { - arm.stopMotor() - } - - override fun stopIntake() { - intake.stopMotor() - } -} diff --git a/src/main/kotlin/org/frc5183/robot/subsystems/drive/SwerveDriveSubsystem.kt b/src/main/kotlin/org/frc5183/robot/subsystems/drive/SwerveDriveSubsystem.kt index eb32796..ce49835 100644 --- a/src/main/kotlin/org/frc5183/robot/subsystems/drive/SwerveDriveSubsystem.kt +++ b/src/main/kotlin/org/frc5183/robot/subsystems/drive/SwerveDriveSubsystem.kt @@ -4,49 +4,49 @@ import com.pathplanner.lib.auto.AutoBuilder import com.pathplanner.lib.commands.PathfindingCommand import com.pathplanner.lib.controllers.PPHolonomicDriveController import com.pathplanner.lib.pathfinding.Pathfinding +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.Translation2d import edu.wpi.first.math.kinematics.ChassisSpeeds import edu.wpi.first.math.kinematics.SwerveDriveKinematics import edu.wpi.first.math.kinematics.SwerveModuleState +import edu.wpi.first.math.numbers.N1 +import edu.wpi.first.math.numbers.N3 +import edu.wpi.first.units.Units import edu.wpi.first.units.measure.Force import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj2.command.CommandScheduler import edu.wpi.first.wpilibj2.command.SubsystemBase import org.frc5183.robot.constants.AutoConstants +import org.frc5183.robot.constants.PhysicalConstants import org.frc5183.robot.constants.swerve.SwerveConstants import org.frc5183.robot.constants.swerve.SwervePIDConstants import org.frc5183.robot.constants.swerve.toPathPlannerPIDConstants import org.frc5183.robot.math.pathfinding.LocalADStarAK -import org.frc5183.robot.subsystems.drive.io.SwerveDriveIO -import org.frc5183.robot.subsystems.drive.io.SwerveDriveIOInputs import org.frc5183.robot.subsystems.vision.VisionSubsystem -import org.littletonrobotics.junction.Logger +import swervelib.SwerveDrive import swervelib.math.SwerveMath import swervelib.telemetry.SwerveDriveTelemetry import kotlin.jvm.optionals.getOrNull class SwerveDriveSubsystem( - private val io: SwerveDriveIO, - private val vision: VisionSubsystem? = null, + val drive: SwerveDrive, + val vision: VisionSubsystem?, ) : SubsystemBase() { - private val ioInputs = SwerveDriveIOInputs() - val robotPose: Pose2d - get() = ioInputs.pose + get() = drive.pose val robotVelocity: ChassisSpeeds - get() = ioInputs.robotVelocity + get() = drive.robotVelocity val fieldVelocity: ChassisSpeeds - get() = ioInputs.fieldVelocity + get() = drive.fieldVelocity val moduleStates: Array - get() = ioInputs.moduleStates + get() = drive.states val kinematics: SwerveDriveKinematics - get() = ioInputs.kinematics + get() = drive.kinematics init { SwerveDriveTelemetry.verbosity = SwerveConstants.VERBOSITY @@ -69,20 +69,35 @@ class SwerveDriveSubsystem( // https://pathplanner.dev/pplib-follow-a-single-path.html#java-warmup CommandScheduler.getInstance().schedule(PathfindingCommand.warmupCommand()) + + if (vision != null) { + drive.stopOdometryThread() + } } override fun periodic() { - io.updateInputs(ioInputs) - Logger.processInputs("Swerve", ioInputs) + if (vision != null) { + vision.frontRobotPose?.let { + addVisionMeasurement(it.estimatedPose.toPose2d(), it.timestampSeconds, vision.frontCamera.currentStandardDeviations) + } - vision?.estimatedRobotPoses?.forEach { (camera, pose) -> - io.addVisionMeasurement(pose.estimatedPose.toPose2d(), pose.timestampSeconds, camera.currentStandardDeviations) + vision.backRobotPose?.let { + addVisionMeasurement(it.estimatedPose.toPose2d(), it.timestampSeconds, vision.backCamera.currentStandardDeviations) + } + + drive.updateOdometry() } } - fun setMotorBrake(brake: Boolean) = io.setMotorBrake(brake) + private fun addVisionMeasurement( + pose: Pose2d, + timestampSeconds: Double, + standardDeviations: Matrix, + ) = drive.addVisionMeasurement(pose, timestampSeconds, standardDeviations) + + fun setMotorBrake(brake: Boolean) = drive.setMotorIdleMode(brake) - fun resetPose(pose: Pose2d = Pose2d.kZero) = io.resetPose(pose) + fun resetPose(pose: Pose2d = Pose2d.kZero) = drive.resetOdometry(pose) fun getTargetSpeeds( x: Double, @@ -91,7 +106,14 @@ class SwerveDriveSubsystem( headingY: Double = 0.0, ): ChassisSpeeds { val scaledInputs = SwerveMath.cubeTranslation(Translation2d(x, y)) - return io.getTargetSpeeds(scaledInputs.x, scaledInputs.y, headingX, headingY) + return drive.swerveController.getTargetSpeeds( + scaledInputs.x, + scaledInputs.y, + headingX, + headingY, + drive.pose.rotation.radians, + PhysicalConstants.MAX_VELOCITY.`in`(Units.MetersPerSecond), + ) } fun drive( @@ -99,15 +121,15 @@ class SwerveDriveSubsystem( rotation: Double, fieldOriented: Boolean, openLoop: Boolean = false, - ) = io.drive(translation, rotation, fieldOriented, openLoop) + ) = drive.drive(translation, rotation, fieldOriented, openLoop) fun drive( robotRelativeVelocity: ChassisSpeeds, states: Array, feedforwardForces: Array, - ) = io.drive(robotRelativeVelocity, states, feedforwardForces) + ) = drive.drive(robotRelativeVelocity, states, feedforwardForces) - fun driveFieldOriented(speeds: ChassisSpeeds) = io.driveFieldOriented(speeds) + fun driveFieldOriented(speeds: ChassisSpeeds) = drive.driveFieldOriented(speeds) - fun driveRobotOriented(speeds: ChassisSpeeds) = io.driveRobotOriented(speeds) + fun driveRobotOriented(speeds: ChassisSpeeds) = drive.drive(speeds) } diff --git a/src/main/kotlin/org/frc5183/robot/subsystems/drive/io/RealSwerveDriveIO.kt b/src/main/kotlin/org/frc5183/robot/subsystems/drive/io/RealSwerveDriveIO.kt deleted file mode 100644 index d7b5b44..0000000 --- a/src/main/kotlin/org/frc5183/robot/subsystems/drive/io/RealSwerveDriveIO.kt +++ /dev/null @@ -1,68 +0,0 @@ -package org.frc5183.robot.subsystems.drive.io - -import edu.wpi.first.math.Matrix -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.SwerveModuleState -import edu.wpi.first.math.numbers.N1 -import edu.wpi.first.math.numbers.N3 -import edu.wpi.first.units.measure.Force -import org.frc5183.robot.constants.swerve.SwerveConstants -import swervelib.SwerveDrive - -open class RealSwerveDriveIO( - private val drive: SwerveDrive, -) : SwerveDriveIO { - init { - drive.headingCorrection = false - drive.setCosineCompensator(SwerveConstants.COSINE_COMPENSATOR) - } - - override fun updateInputs(inputs: SwerveDriveIOInputs) { - inputs.pose = drive.pose - inputs.robotVelocity = drive.robotVelocity - inputs.fieldVelocity = drive.fieldVelocity - inputs.moduleStates = drive.states - inputs.kinematics = drive.kinematics - } - - override fun stopOdometryThread() = drive.stopOdometryThread() - - override fun updateOdometry() = drive.updateOdometry() - - override fun addVisionMeasurement( - pose: Pose2d, - timestampSeconds: Double, - standardDeviations: Matrix, - ) = drive.addVisionMeasurement(pose, timestampSeconds, standardDeviations) - - override fun setMotorBrake(brake: Boolean) = drive.setMotorIdleMode(brake) - - override fun resetPose(pose: Pose2d) = drive.resetOdometry(pose) - - override fun getTargetSpeeds( - x: Double, - y: Double, - headingX: Double, - headingY: Double, - ): ChassisSpeeds = - drive.swerveController.getTargetSpeeds(x, y, headingX, headingY, drive.pose.rotation.radians, TODO("requires constants")) - - override fun drive( - translation: Translation2d, - rotation: Double, - fieldOriented: Boolean, - openLoop: Boolean, - ) = drive.drive(translation, rotation, fieldOriented, openLoop) - - override fun drive( - robotRelativeVelocity: ChassisSpeeds, - states: Array, - feedforwardForces: Array, - ) = drive.drive(robotRelativeVelocity, states, feedforwardForces) - - override fun driveFieldOriented(speeds: ChassisSpeeds) = drive.driveFieldOriented(speeds) - - override fun driveRobotOriented(speeds: ChassisSpeeds) = drive.drive(speeds) -} diff --git a/src/main/kotlin/org/frc5183/robot/subsystems/drive/io/SimulatedSwerveDriveIO.kt b/src/main/kotlin/org/frc5183/robot/subsystems/drive/io/SimulatedSwerveDriveIO.kt deleted file mode 100644 index f669edd..0000000 --- a/src/main/kotlin/org/frc5183/robot/subsystems/drive/io/SimulatedSwerveDriveIO.kt +++ /dev/null @@ -1,12 +0,0 @@ -package org.frc5183.robot.subsystems.drive.io - -import swervelib.SwerveDrive - -class SimulatedSwerveDriveIO( - drive: SwerveDrive, -) : RealSwerveDriveIO(drive) { - init { - drive.headingCorrection = false - drive.setCosineCompensator(false) - } -} diff --git a/src/main/kotlin/org/frc5183/robot/subsystems/drive/io/SwerveDriveIO.kt b/src/main/kotlin/org/frc5183/robot/subsystems/drive/io/SwerveDriveIO.kt deleted file mode 100644 index 52b06f0..0000000 --- a/src/main/kotlin/org/frc5183/robot/subsystems/drive/io/SwerveDriveIO.kt +++ /dev/null @@ -1,104 +0,0 @@ -package org.frc5183.robot.subsystems.drive.io - -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.Translation2d -import edu.wpi.first.math.kinematics.ChassisSpeeds -import edu.wpi.first.math.kinematics.SwerveModuleState -import edu.wpi.first.math.numbers.N1 -import edu.wpi.first.math.numbers.N3 -import edu.wpi.first.units.measure.Force - -interface SwerveDriveIO { - /** - * Updates the IO inputs with current values to be logged. - * - * @param inputs The input object to be updated. - */ - fun updateInputs(inputs: SwerveDriveIOInputs) - - /** - * Stops YAGSL's odometry thread. - * If this is stopped then [updateOdometry] must be called. - */ - fun stopOdometryThread() - - /** - * Adds a vision measurement to the swerve drive. - * @param pose The pose to add. - * @param timestampSeconds The timestamp of the measurement. - * @param standardDeviations The standard deviations of the measurement. - */ - fun addVisionMeasurement( - pose: Pose2d, - timestampSeconds: Double, - standardDeviations: Matrix, - ) - - /** - * Updates the robot's odometry using YAGSL. - * This should only be run when the odometry thread is stopped (which should - * only be when vision pose estimation is enabled). - */ - fun updateOdometry() - - /** - * Sets the drive motors to brake/coast mode. - * @param brake Whether to set the motors to brake mode (true) or coast mode (false). - */ - fun setMotorBrake(brake: Boolean) - - /** - * Resets the robot's pose to [pose]. - */ - fun resetPose(pose: Pose2d = Pose2d.kZero) - - /** - * Get the chassis speeds based on controller input of 2 joysticks - */ - fun getTargetSpeeds( - x: Double, - y: Double, - headingX: Double, - headingY: Double, - ): ChassisSpeeds - - /** - * Drives the robot with the given translation and rotation. - * @param translation The translation to drive with. - * @param rotation The rotation to drive with. - * @param fieldOriented Whether the translation is field-oriented. - * @param openLoop Whether the drive is open-loop. - */ - fun drive( - translation: Translation2d, - rotation: Double, - fieldOriented: Boolean, - openLoop: Boolean, - ) - - /** - * Drive the robot using [SwerveModuleState]s. - * @param robotRelativeVelocity The robot relative velocity to drive with. - * @param states The swerve module states to drive with. - * @param feedforwardForces The feedforward forces to apply to each module. - */ - fun drive( - robotRelativeVelocity: ChassisSpeeds, - states: Array, - feedforwardForces: Array, - ) - - /** - * Drive the robot given a chassis field oriented velocity. - * @param speeds Field oriented chassis speeds to drive with. - */ - fun driveFieldOriented(speeds: ChassisSpeeds) - - /** - * Drive the robot given a chassis robot oriented velocity. - * @param speeds Robot oriented chassis speeds to drive with. - */ - fun driveRobotOriented(speeds: ChassisSpeeds) -} diff --git a/src/main/kotlin/org/frc5183/robot/subsystems/drive/io/SwerveDriveIOInputs.kt b/src/main/kotlin/org/frc5183/robot/subsystems/drive/io/SwerveDriveIOInputs.kt deleted file mode 100644 index 381bbc9..0000000 --- a/src/main/kotlin/org/frc5183/robot/subsystems/drive/io/SwerveDriveIOInputs.kt +++ /dev/null @@ -1,15 +0,0 @@ -package org.frc5183.robot.subsystems.drive.io - -import edu.wpi.first.math.geometry.Pose2d -import edu.wpi.first.math.kinematics.ChassisSpeeds -import edu.wpi.first.math.kinematics.SwerveDriveKinematics -import edu.wpi.first.math.kinematics.SwerveModuleState -import org.frc5183.robot.logging.AutoLogInputs - -class SwerveDriveIOInputs : AutoLogInputs() { - var pose by log(Pose2d()) - var robotVelocity by log(ChassisSpeeds()) - var fieldVelocity by log(ChassisSpeeds()) - var moduleStates by log(emptyArray()) - var kinematics by log(SwerveDriveKinematics()) -} diff --git a/src/main/kotlin/org/frc5183/robot/subsystems/shooter/ShooterSubsystem.kt b/src/main/kotlin/org/frc5183/robot/subsystems/shooter/ShooterSubsystem.kt index 066cc37..86b1df6 100644 --- a/src/main/kotlin/org/frc5183/robot/subsystems/shooter/ShooterSubsystem.kt +++ b/src/main/kotlin/org/frc5183/robot/subsystems/shooter/ShooterSubsystem.kt @@ -1,25 +1,18 @@ package org.frc5183.robot.subsystems.shooter +import com.revrobotics.spark.SparkMax import edu.wpi.first.wpilibj2.command.SubsystemBase -import org.frc5183.robot.subsystems.shooter.io.ShooterIO -import org.frc5183.robot.subsystems.shooter.io.ShooterIOInputs +import org.littletonrobotics.junction.Logger class ShooterSubsystem( - private val io: ShooterIO, + val shooter: SparkMax, + val intake: SparkMax, + val feeder: SparkMax, ) : SubsystemBase() { - private val ioInputs = ShooterIOInputs() - - val shooterSpeed: Double - get() = ioInputs.shooterSpeed - - val intakeSpeed: Double - get() = ioInputs.intakeSpeed - - val feederSpeed: Double - get() = ioInputs.feederSpeed - override fun periodic() { - io.updateInputs(ioInputs) + Logger.recordOutput("Shooter/ShooterSpeed", shooter.get()) + Logger.recordOutput("Shooter/IntakeSpeed", intake.get()) + Logger.recordOutput("Shooter/FeederSpeed", feeder.get()) } fun run(speed: Double) { @@ -28,11 +21,11 @@ class ShooterSubsystem( runFeeder(speed) } - fun runShooter(speed: Double) = io.runShooter(speed) + fun runShooter(speed: Double) = shooter.set(speed) - fun runIntake(speed: Double) = io.runIntake(speed) + fun runIntake(speed: Double) = intake.set(speed) - fun runFeeder(speed: Double) = io.runFeeder(speed) + fun runFeeder(speed: Double) = feeder.set(speed) fun stop() { stopShooter() @@ -40,9 +33,9 @@ class ShooterSubsystem( stopFeeder() } - fun stopShooter() = io.stopShooter() + fun stopShooter() = shooter.stopMotor() - fun stopIntake() = io.stopIntake() + fun stopIntake() = intake.stopMotor() - fun stopFeeder() = io.stopFeeder() + fun stopFeeder() = feeder.stopMotor() } diff --git a/src/main/kotlin/org/frc5183/robot/subsystems/shooter/io/RealShooterIO.kt b/src/main/kotlin/org/frc5183/robot/subsystems/shooter/io/RealShooterIO.kt deleted file mode 100644 index 6cab63b..0000000 --- a/src/main/kotlin/org/frc5183/robot/subsystems/shooter/io/RealShooterIO.kt +++ /dev/null @@ -1,27 +0,0 @@ -package org.frc5183.robot.subsystems.shooter.io - -import com.revrobotics.spark.SparkMax - -class RealShooterIO( - val shooter: SparkMax, - val intake: SparkMax, - val feeder: SparkMax, -) : ShooterIO { - override fun updateInputs(inputs: ShooterIOInputs) { - inputs.shooterSpeed = shooter.get() - inputs.intakeSpeed = intake.get() - inputs.feederSpeed = feeder.get() - } - - override fun runShooter(speed: Double) = shooter.set(speed) - - override fun runIntake(speed: Double) = intake.set(speed) - - override fun runFeeder(speed: Double) = feeder.set(speed) - - override fun stopShooter() = shooter.stopMotor() - - override fun stopFeeder() = feeder.stopMotor() - - override fun stopIntake() = intake.stopMotor() -} diff --git a/src/main/kotlin/org/frc5183/robot/subsystems/shooter/io/ShooterIO.kt b/src/main/kotlin/org/frc5183/robot/subsystems/shooter/io/ShooterIO.kt deleted file mode 100644 index 0cd2fe4..0000000 --- a/src/main/kotlin/org/frc5183/robot/subsystems/shooter/io/ShooterIO.kt +++ /dev/null @@ -1,17 +0,0 @@ -package org.frc5183.robot.subsystems.shooter.io - -interface ShooterIO { - fun updateInputs(inputs: ShooterIOInputs) - - fun runShooter(speed: Double) - - fun runIntake(speed: Double) - - fun runFeeder(speed: Double) - - fun stopShooter() - - fun stopIntake() - - fun stopFeeder() -} diff --git a/src/main/kotlin/org/frc5183/robot/subsystems/shooter/io/ShooterIOInputs.kt b/src/main/kotlin/org/frc5183/robot/subsystems/shooter/io/ShooterIOInputs.kt deleted file mode 100644 index 45da61a..0000000 --- a/src/main/kotlin/org/frc5183/robot/subsystems/shooter/io/ShooterIOInputs.kt +++ /dev/null @@ -1,9 +0,0 @@ -package org.frc5183.robot.subsystems.shooter.io - -import org.frc5183.robot.logging.AutoLogInputs - -class ShooterIOInputs : AutoLogInputs() { - var shooterSpeed by log(0.0) - var intakeSpeed by log(0.0) - var feederSpeed by log(0.0) -} diff --git a/src/main/kotlin/org/frc5183/robot/subsystems/turntable/TurntableSubsystem.kt b/src/main/kotlin/org/frc5183/robot/subsystems/turntable/TurntableSubsystem.kt index 251b9f4..fc0e3be 100644 --- a/src/main/kotlin/org/frc5183/robot/subsystems/turntable/TurntableSubsystem.kt +++ b/src/main/kotlin/org/frc5183/robot/subsystems/turntable/TurntableSubsystem.kt @@ -1,27 +1,32 @@ package org.frc5183.robot.subsystems.turntable +import com.revrobotics.spark.SparkMax import edu.wpi.first.wpilibj2.command.SubsystemBase -import org.frc5183.robot.subsystems.turntable.io.TurntableIO -import org.frc5183.robot.subsystems.turntable.io.TurntableIOInputs +import org.littletonrobotics.junction.Logger +import org.photonvision.PhotonCamera import org.photonvision.targeting.PhotonTrackedTarget class TurntableSubsystem( - val io: TurntableIO, + val motor: SparkMax, + val camera: PhotonCamera, ) : SubsystemBase() { - val ioInputs = TurntableIOInputs() - val targets: Array - get() = ioInputs.targets + get() = + camera.allUnreadResults + .map { it.targets } + .flatten() + .toTypedArray() override fun periodic() { - io.updateInputs(ioInputs) + Logger.recordOutput("Turntable/Speed", motor.get()) + Logger.recordOutput("Turntable/Targets", targets.map { it.fiducialId }.toIntArray()) } fun setSpeed(speed: Double) { - io.setSpeed(speed) + motor.set(speed) } fun stop() { - io.stop() + motor.stopMotor() } } diff --git a/src/main/kotlin/org/frc5183/robot/subsystems/turntable/io/RealTurntableIO.kt b/src/main/kotlin/org/frc5183/robot/subsystems/turntable/io/RealTurntableIO.kt deleted file mode 100644 index 89b73ad..0000000 --- a/src/main/kotlin/org/frc5183/robot/subsystems/turntable/io/RealTurntableIO.kt +++ /dev/null @@ -1,28 +0,0 @@ -package org.frc5183.robot.subsystems.turntable.io - -import com.revrobotics.spark.SparkMax -import org.photonvision.PhotonCamera -import org.photonvision.targeting.PhotonTrackedTarget - -class RealTurntableIO( - val motor: SparkMax, - val camera: PhotonCamera, -) : TurntableIO { - override fun updateInputs(inputs: TurntableIOInputs) { - inputs.speed = motor.get() - - inputs.targets = - camera.allUnreadResults - .map { it.targets } - .flatten() - .toTypedArray() - } - - override fun setSpeed(speed: Double) { - motor.set(speed) - } - - override fun stop() { - motor.stopMotor() - } -} diff --git a/src/main/kotlin/org/frc5183/robot/subsystems/turntable/io/TurntableIO.kt b/src/main/kotlin/org/frc5183/robot/subsystems/turntable/io/TurntableIO.kt deleted file mode 100644 index 80b42f8..0000000 --- a/src/main/kotlin/org/frc5183/robot/subsystems/turntable/io/TurntableIO.kt +++ /dev/null @@ -1,11 +0,0 @@ -package org.frc5183.robot.subsystems.turntable.io - -import org.photonvision.targeting.PhotonTrackedTarget - -interface TurntableIO { - fun updateInputs(inputs: TurntableIOInputs) - - fun setSpeed(speed: Double) - - fun stop() -} diff --git a/src/main/kotlin/org/frc5183/robot/subsystems/turntable/io/TurntableIOInputs.kt b/src/main/kotlin/org/frc5183/robot/subsystems/turntable/io/TurntableIOInputs.kt deleted file mode 100644 index 132f3ea..0000000 --- a/src/main/kotlin/org/frc5183/robot/subsystems/turntable/io/TurntableIOInputs.kt +++ /dev/null @@ -1,10 +0,0 @@ -package org.frc5183.robot.subsystems.turntable.io - -import org.frc5183.robot.logging.AutoLogInputs -import org.frc5183.robot.logging.struct.PhotonTrackedTargetStruct -import org.photonvision.targeting.PhotonTrackedTarget - -class TurntableIOInputs : AutoLogInputs() { - var speed by log(0.0) - var targets by log(emptyArray(), PhotonTrackedTargetStruct) -} diff --git a/src/main/kotlin/org/frc5183/robot/subsystems/vision/VisionSubsystem.kt b/src/main/kotlin/org/frc5183/robot/subsystems/vision/VisionSubsystem.kt index cf933f5..f9004a9 100644 --- a/src/main/kotlin/org/frc5183/robot/subsystems/vision/VisionSubsystem.kt +++ b/src/main/kotlin/org/frc5183/robot/subsystems/vision/VisionSubsystem.kt @@ -1,31 +1,34 @@ package org.frc5183.robot.subsystems.vision import edu.wpi.first.wpilibj2.command.SubsystemBase -import org.frc5183.robot.constants.VisionConstants -import org.frc5183.robot.subsystems.vision.io.VisionIO -import org.frc5183.robot.subsystems.vision.io.VisionIOInputs +import org.littletonrobotics.junction.Logger import org.photonvision.EstimatedRobotPose import org.photonvision.targeting.PhotonTrackedTarget class VisionSubsystem( - val io: VisionIO, + val frontCamera: FixedCamera, + val backCamera: FixedCamera, ) : SubsystemBase() { - private val inputs = VisionIOInputs() + val frontTargets: List + get() = frontCamera.targets - val targets: List - get() = inputs.targets.toList() + val backTargets: List + get() = backCamera.targets - val estimatedRobotPoses: Map - get() { - val map = mutableMapOf() + val frontRobotPose: EstimatedRobotPose? + get() = frontCamera.estimatedRobotPose - inputs.frontEstimatedRobotPose?.let { map += Pair(VisionConstants.FRONT_CAMERA, it) } - inputs.backEstimatedRobotPose?.let { map += Pair(VisionConstants.BACK_CAMERA, it) } - - return map - } + val backRobotPose: EstimatedRobotPose? + get() = backCamera.estimatedRobotPose override fun periodic() { - io.updateInputs(inputs) + frontCamera.periodic() + backCamera.periodic() + + Logger.recordOutput("Vision/Targets/Front", frontTargets.map { it.fiducialId }.toIntArray()) + Logger.recordOutput("Vision/Targets/Back", backTargets.map { it.fiducialId }.toIntArray()) + + Logger.recordOutput("Vision/EstimatedRobotPoses/Front", frontCamera.estimatedRobotPose?.estimatedPose?.toPose2d()) + Logger.recordOutput("Vision/EstimatedRobotPoses/Back", backCamera.estimatedRobotPose?.estimatedPose?.toPose2d()) } } diff --git a/src/main/kotlin/org/frc5183/robot/subsystems/vision/io/RealVisionIO.kt b/src/main/kotlin/org/frc5183/robot/subsystems/vision/io/RealVisionIO.kt deleted file mode 100644 index 1f49ba1..0000000 --- a/src/main/kotlin/org/frc5183/robot/subsystems/vision/io/RealVisionIO.kt +++ /dev/null @@ -1,20 +0,0 @@ -package org.frc5183.robot.subsystems.vision.io - -import org.frc5183.robot.subsystems.vision.FixedCamera -import org.photonvision.EstimatedRobotPose - -class RealVisionIO( - val frontCamera: FixedCamera, - val backCamera: FixedCamera, -) : VisionIO { - override fun updateInputs(inputs: VisionIOInputs) { - frontCamera.periodic() - backCamera.periodic() - - val targets = frontCamera.targets + backCamera.targets - inputs.targets = targets.toTypedArray() - - inputs.frontEstimatedRobotPose = frontCamera.estimatedRobotPose - inputs.backEstimatedRobotPose = backCamera.estimatedRobotPose - } -} diff --git a/src/main/kotlin/org/frc5183/robot/subsystems/vision/io/VisionIO.kt b/src/main/kotlin/org/frc5183/robot/subsystems/vision/io/VisionIO.kt deleted file mode 100644 index 2a0c81f..0000000 --- a/src/main/kotlin/org/frc5183/robot/subsystems/vision/io/VisionIO.kt +++ /dev/null @@ -1,5 +0,0 @@ -package org.frc5183.robot.subsystems.vision.io - -interface VisionIO { - fun updateInputs(inputs: VisionIOInputs) -} diff --git a/src/main/kotlin/org/frc5183/robot/subsystems/vision/io/VisionIOInputs.kt b/src/main/kotlin/org/frc5183/robot/subsystems/vision/io/VisionIOInputs.kt deleted file mode 100644 index d666e61..0000000 --- a/src/main/kotlin/org/frc5183/robot/subsystems/vision/io/VisionIOInputs.kt +++ /dev/null @@ -1,13 +0,0 @@ -package org.frc5183.robot.subsystems.vision.io - -import org.frc5183.robot.logging.AutoLogInputs -import org.frc5183.robot.logging.struct.EstimatedRobotPoseStruct -import org.frc5183.robot.logging.struct.PhotonTrackedTargetStruct -import org.photonvision.EstimatedRobotPose -import org.photonvision.targeting.PhotonTrackedTarget - -class VisionIOInputs : AutoLogInputs() { - var targets by log(emptyArray(), PhotonTrackedTargetStruct) - var frontEstimatedRobotPose by log(null as EstimatedRobotPose?, EstimatedRobotPoseStruct) - var backEstimatedRobotPose by log(null as EstimatedRobotPose?, EstimatedRobotPoseStruct) -}