From eec3382675e9d9e94805f66069c297db4faa3e65 Mon Sep 17 00:00:00 2001 From: Micah Rao Date: Thu, 16 Nov 2023 19:51:22 -0500 Subject: [PATCH 1/6] Created LimelightSubsystem and added basic NetworkTable stuff --- .../robot/subsystems/LimelightSubsystem.kt | 47 +++++++++++++++++++ 1 file changed, 47 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/LimelightSubsystem.kt diff --git a/src/main/java/frc/robot/subsystems/LimelightSubsystem.kt b/src/main/java/frc/robot/subsystems/LimelightSubsystem.kt new file mode 100644 index 00000000..b6b37691 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LimelightSubsystem.kt @@ -0,0 +1,47 @@ +package frc.robot.subsystems + +import edu.wpi.first.networktables.NetworkTable +import edu.wpi.first.networktables.NetworkTableEntry +import edu.wpi.first.networktables.NetworkTableInstance +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +object LimelightSubsystem : SubsystemBase() { + + private var tx: NetworkTableEntry + private var ty: NetworkTableEntry + private var ta: NetworkTableEntry + private var tv: NetworkTableEntry + private var ts: NetworkTableEntry + + private var table: NetworkTable + + private var xOffset: Double = 0.0 + private var yOffset: Double = 0.0 + private var area: Double = 0.0 + private var skew: Double = 0.0 + private var targetVisible: Boolean = false + + init { + table = NetworkTableInstance.getDefault().getTable("limelight") + + tx = table.getEntry("tx") + ty = table.getEntry("ty") + ta = table.getEntry("ta") + tv = table.getEntry("tv") + ts = table.getEntry("ts") + + val visionTab: ShuffleboardTab = Shuffleboard.getTab("Vision") + + + } + + override fun periodic() { + xOffset = tx.getDouble(0.0) + yOffset = ty.getDouble(0.0) + area = ta.getDouble(0.0) + skew = ts.getDouble(0.0) + targetVisible = tv.getBoolean(false) + } +} \ No newline at end of file From a460a03e86fd31443af08ddf3721adb2fac133d4 Mon Sep 17 00:00:00 2001 From: Falon <47285315+falOn-Dev@users.noreply.github.com> Date: Thu, 16 Nov 2023 20:38:29 -0500 Subject: [PATCH 2/6] added getters for the limelight variables --- .../robot/subsystems/LimelightSubsystem.kt | 30 ++++++++++++++++++- 1 file changed, 29 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/LimelightSubsystem.kt b/src/main/java/frc/robot/subsystems/LimelightSubsystem.kt index b6b37691..c00ae6b9 100644 --- a/src/main/java/frc/robot/subsystems/LimelightSubsystem.kt +++ b/src/main/java/frc/robot/subsystems/LimelightSubsystem.kt @@ -9,14 +9,17 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; object LimelightSubsystem : SubsystemBase() { + // NetworkTableEntry objects for getting data from the Limelight private var tx: NetworkTableEntry private var ty: NetworkTableEntry private var ta: NetworkTableEntry private var tv: NetworkTableEntry private var ts: NetworkTableEntry + // NetworkTable object for getting data from the Limelight private var table: NetworkTable + // Variables for storing data from the Limelight private var xOffset: Double = 0.0 private var yOffset: Double = 0.0 private var area: Double = 0.0 @@ -24,24 +27,49 @@ object LimelightSubsystem : SubsystemBase() { private var targetVisible: Boolean = false init { + // Get the default NetworkTable for the Limelight table = NetworkTableInstance.getDefault().getTable("limelight") + // Get the NetworkTableEntry objects for the Limelight tx = table.getEntry("tx") ty = table.getEntry("ty") ta = table.getEntry("ta") tv = table.getEntry("tv") ts = table.getEntry("ts") + // Create a Shuffleboard tab for the Limelight val visionTab: ShuffleboardTab = Shuffleboard.getTab("Vision") } override fun periodic() { + // Update the data with the latest values from the Limelight xOffset = tx.getDouble(0.0) yOffset = ty.getDouble(0.0) area = ta.getDouble(0.0) skew = ts.getDouble(0.0) - targetVisible = tv.getBoolean(false) + targetVisible = tv.getDouble(0.0) == 1.0 + } + + // Getters for the data from the Limelight + fun getXOffset(): Double { + return xOffset + } + + fun getYOffset(): Double { + return yOffset + } + + fun getArea(): Double { + return area + } + + fun getSkew(): Double { + return skew + } + + fun isTargetVisible(): Boolean { + return targetVisible } } \ No newline at end of file From 9f48b0580ef2d40ece4d609e676c8dfe2cb883f3 Mon Sep 17 00:00:00 2001 From: Micah Rao Date: Thu, 30 Nov 2023 18:38:33 -0500 Subject: [PATCH 3/6] stop forgorring fallon --- src/main/java/frc/robot/RobotContainer.kt | 13 ++++- .../commands/vision/TrackTargetCommand.kt | 50 +++++++++++++++++++ .../robot/subsystems/LimelightSubsystem.kt | 23 ++++++--- .../frc/robot/subsystems/SwerveSubsystem.kt | 3 -- 4 files changed, 77 insertions(+), 12 deletions(-) create mode 100644 src/main/java/frc/robot/commands/vision/TrackTargetCommand.kt diff --git a/src/main/java/frc/robot/RobotContainer.kt b/src/main/java/frc/robot/RobotContainer.kt index c48dc2fc..64c96bd9 100644 --- a/src/main/java/frc/robot/RobotContainer.kt +++ b/src/main/java/frc/robot/RobotContainer.kt @@ -8,7 +8,9 @@ import frc.robot.Constants.OperatorConstants import frc.robot.commands.Autos import frc.robot.commands.ExampleCommand import frc.robot.commands.swerve.TeleopDrive +import frc.robot.commands.vision.TrackTargetCommand import frc.robot.subsystems.ExampleSubsystem +import frc.robot.subsystems.LimelightSubsystem import frc.robot.util.SingletonXboxController /** @@ -26,6 +28,11 @@ object RobotContainer { val controller = SingletonXboxController + val limelight = LimelightSubsystem + val drivebase = SwerveSubsystem + + val trackTarget = TrackTargetCommand() + // TODO: This is kinda weird but inverting (and the drive encoders) makes it display properly // No, uninverting both doesn't fix it :( val teleopDrive: TeleopDrive = TeleopDrive( @@ -40,7 +47,7 @@ object RobotContainer { init { configureBindings() - SwerveSubsystem.defaultCommand = teleopDrive + drivebase.defaultCommand = teleopDrive // Reference the Autos object so that it is initialized, placing the chooser on the dashboard Autos } @@ -54,6 +61,8 @@ object RobotContainer { * controllers or [Flight joysticks][edu.wpi.first.wpilibj2.command.button.CommandJoystick]. */ private fun configureBindings() { - controller.b().onTrue(SwerveSubsystem.runOnce { SwerveSubsystem.zeroGyro() }) + controller.b().onTrue(drivebase.runOnce { drivebase.zeroGyro() }) + + controller.a().toggleOnTrue(trackTarget) } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/vision/TrackTargetCommand.kt b/src/main/java/frc/robot/commands/vision/TrackTargetCommand.kt new file mode 100644 index 00000000..a9a42317 --- /dev/null +++ b/src/main/java/frc/robot/commands/vision/TrackTargetCommand.kt @@ -0,0 +1,50 @@ +package frc.robot.commands.vision + +import SwerveSubsystem +import edu.wpi.first.math.controller.PIDController +import edu.wpi.first.math.geometry.Translation2d +import edu.wpi.first.math.geometry.Translation3d +import edu.wpi.first.wpilibj2.command.CommandBase +import frc.robot.subsystems.LimelightSubsystem +import frc.robot.util.SingletonXboxController +import kotlin.math.abs + +class TrackTargetCommand : CommandBase() { + private val limelightSubsystem = LimelightSubsystem + private val drivebase = SwerveSubsystem + private val pidController: PIDController + + private var rotation: Double = 0.0 + private var translation: Translation2d = Translation2d(0.0, 0.0) + + + init { + // each subsystem used by the command must be passed into the addRequirements() method + addRequirements(limelightSubsystem, drivebase) + pidController = PIDController(0.1, 0.0, 0.0) + + + } + + override fun initialize() {} + + override fun execute() { + rotation = pidController.calculate(limelightSubsystem.xOffset, 0.0) + + + + if((abs(limelightSubsystem.xOffset) < 2 && limelightSubsystem.area < 3.5) && limelightSubsystem.targetVisible){ + translation = Translation2d(0.3, -SingletonXboxController.leftX) + } else { + translation = Translation2d(0.0, -SingletonXboxController.leftX) + } + drivebase.drive(translation, rotation, false) + } + + override fun isFinished(): Boolean { + // TODO: Make this return true when this Command no longer needs to run execute() + return false + } + + override fun end(interrupted: Boolean) {} +} diff --git a/src/main/java/frc/robot/subsystems/LimelightSubsystem.kt b/src/main/java/frc/robot/subsystems/LimelightSubsystem.kt index b6b37691..69c5f2df 100644 --- a/src/main/java/frc/robot/subsystems/LimelightSubsystem.kt +++ b/src/main/java/frc/robot/subsystems/LimelightSubsystem.kt @@ -17,11 +17,13 @@ object LimelightSubsystem : SubsystemBase() { private var table: NetworkTable - private var xOffset: Double = 0.0 - private var yOffset: Double = 0.0 - private var area: Double = 0.0 - private var skew: Double = 0.0 - private var targetVisible: Boolean = false + var xOffset: Double = 0.0 + var yOffset: Double = 0.0 + var area: Double = 0.0 + var skew: Double = 0.0 + var targetVisible: Boolean = false + + private var visionTab: ShuffleboardTab init { table = NetworkTableInstance.getDefault().getTable("limelight") @@ -32,7 +34,14 @@ object LimelightSubsystem : SubsystemBase() { tv = table.getEntry("tv") ts = table.getEntry("ts") - val visionTab: ShuffleboardTab = Shuffleboard.getTab("Vision") + visionTab = Shuffleboard.getTab("Vision") + + visionTab.addDouble("X Offset") { xOffset } + visionTab.addDouble("Y Offset") { yOffset } + visionTab.addDouble("Area") { area } + visionTab.addDouble("Skew") { skew } + visionTab.addBoolean("Target Visible") { targetVisible } + } @@ -42,6 +51,6 @@ object LimelightSubsystem : SubsystemBase() { yOffset = ty.getDouble(0.0) area = ta.getDouble(0.0) skew = ts.getDouble(0.0) - targetVisible = tv.getBoolean(false) + targetVisible = tv.getDouble(0.0) > 0 } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.kt b/src/main/java/frc/robot/subsystems/SwerveSubsystem.kt index ebf4fe89..8ffadf1f 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.kt +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.kt @@ -19,7 +19,6 @@ import swervelib.parser.SwerveDriveConfiguration import swervelib.parser.SwerveParser import swervelib.telemetry.SwerveDriveTelemetry - object SwerveSubsystem : SubsystemBase() { private val swerveDrive: SwerveDrive private var autoBuilder: SwerveAutoBuilder? = null @@ -135,6 +134,4 @@ object SwerveSubsystem : SubsystemBase() { getSwerveDriveConfiguration()?.modules?.get(3)?.driveMotor?.velocity ?: 0.0) } - - } From a2229cb9ae0e841d9d46735c0674d0a6a8f45095 Mon Sep 17 00:00:00 2001 From: Falon <47285315+falOn-Dev@users.noreply.github.com> Date: Sun, 10 Dec 2023 13:46:51 -0500 Subject: [PATCH 4/6] Revert "Update LimelightSubsystem.kt" This reverts commit bda631206a6c1d3a246e3377e513b840abe57d34, reversing changes made to 9f48b0580ef2d40ece4d609e676c8dfe2cb883f3. --- .../robot/subsystems/LimelightSubsystem.kt | 44 ------------------- 1 file changed, 44 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/LimelightSubsystem.kt b/src/main/java/frc/robot/subsystems/LimelightSubsystem.kt index ec5cf80a..69c5f2df 100644 --- a/src/main/java/frc/robot/subsystems/LimelightSubsystem.kt +++ b/src/main/java/frc/robot/subsystems/LimelightSubsystem.kt @@ -9,17 +9,14 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; object LimelightSubsystem : SubsystemBase() { - // NetworkTableEntry objects for getting data from the Limelight private var tx: NetworkTableEntry private var ty: NetworkTableEntry private var ta: NetworkTableEntry private var tv: NetworkTableEntry private var ts: NetworkTableEntry - // NetworkTable object for getting data from the Limelight private var table: NetworkTable -<<<<<<< HEAD var xOffset: Double = 0.0 var yOffset: Double = 0.0 var area: Double = 0.0 @@ -27,27 +24,16 @@ object LimelightSubsystem : SubsystemBase() { var targetVisible: Boolean = false private var visionTab: ShuffleboardTab -======= - // Variables for storing data from the Limelight - private var xOffset: Double = 0.0 - private var yOffset: Double = 0.0 - private var area: Double = 0.0 - private var skew: Double = 0.0 - private var targetVisible: Boolean = false ->>>>>>> a460a03e86fd31443af08ddf3721adb2fac133d4 init { - // Get the default NetworkTable for the Limelight table = NetworkTableInstance.getDefault().getTable("limelight") - // Get the NetworkTableEntry objects for the Limelight tx = table.getEntry("tx") ty = table.getEntry("ty") ta = table.getEntry("ta") tv = table.getEntry("tv") ts = table.getEntry("ts") -<<<<<<< HEAD visionTab = Shuffleboard.getTab("Vision") visionTab.addDouble("X Offset") { xOffset } @@ -56,45 +42,15 @@ object LimelightSubsystem : SubsystemBase() { visionTab.addDouble("Skew") { skew } visionTab.addBoolean("Target Visible") { targetVisible } -======= - // Create a Shuffleboard tab for the Limelight - val visionTab: ShuffleboardTab = Shuffleboard.getTab("Vision") ->>>>>>> a460a03e86fd31443af08ddf3721adb2fac133d4 } override fun periodic() { - // Update the data with the latest values from the Limelight xOffset = tx.getDouble(0.0) yOffset = ty.getDouble(0.0) area = ta.getDouble(0.0) skew = ts.getDouble(0.0) -<<<<<<< HEAD targetVisible = tv.getDouble(0.0) > 0 -======= - targetVisible = tv.getDouble(0.0) == 1.0 - } - - // Getters for the data from the Limelight - fun getXOffset(): Double { - return xOffset - } - - fun getYOffset(): Double { - return yOffset - } - - fun getArea(): Double { - return area - } - - fun getSkew(): Double { - return skew - } - - fun isTargetVisible(): Boolean { - return targetVisible ->>>>>>> a460a03e86fd31443af08ddf3721adb2fac133d4 } } \ No newline at end of file From 28121121c5e435801a8face6599a6ae949d8f2c4 Mon Sep 17 00:00:00 2001 From: Falon <47285315+falOn-Dev@users.noreply.github.com> Date: Sun, 10 Dec 2023 13:47:16 -0500 Subject: [PATCH 5/6] Revert "Revert "Update LimelightSubsystem.kt"" This reverts commit a2229cb9ae0e841d9d46735c0674d0a6a8f45095. --- .../robot/subsystems/LimelightSubsystem.kt | 44 +++++++++++++++++++ 1 file changed, 44 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/LimelightSubsystem.kt b/src/main/java/frc/robot/subsystems/LimelightSubsystem.kt index 69c5f2df..ec5cf80a 100644 --- a/src/main/java/frc/robot/subsystems/LimelightSubsystem.kt +++ b/src/main/java/frc/robot/subsystems/LimelightSubsystem.kt @@ -9,14 +9,17 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; object LimelightSubsystem : SubsystemBase() { + // NetworkTableEntry objects for getting data from the Limelight private var tx: NetworkTableEntry private var ty: NetworkTableEntry private var ta: NetworkTableEntry private var tv: NetworkTableEntry private var ts: NetworkTableEntry + // NetworkTable object for getting data from the Limelight private var table: NetworkTable +<<<<<<< HEAD var xOffset: Double = 0.0 var yOffset: Double = 0.0 var area: Double = 0.0 @@ -24,16 +27,27 @@ object LimelightSubsystem : SubsystemBase() { var targetVisible: Boolean = false private var visionTab: ShuffleboardTab +======= + // Variables for storing data from the Limelight + private var xOffset: Double = 0.0 + private var yOffset: Double = 0.0 + private var area: Double = 0.0 + private var skew: Double = 0.0 + private var targetVisible: Boolean = false +>>>>>>> a460a03e86fd31443af08ddf3721adb2fac133d4 init { + // Get the default NetworkTable for the Limelight table = NetworkTableInstance.getDefault().getTable("limelight") + // Get the NetworkTableEntry objects for the Limelight tx = table.getEntry("tx") ty = table.getEntry("ty") ta = table.getEntry("ta") tv = table.getEntry("tv") ts = table.getEntry("ts") +<<<<<<< HEAD visionTab = Shuffleboard.getTab("Vision") visionTab.addDouble("X Offset") { xOffset } @@ -42,15 +56,45 @@ object LimelightSubsystem : SubsystemBase() { visionTab.addDouble("Skew") { skew } visionTab.addBoolean("Target Visible") { targetVisible } +======= + // Create a Shuffleboard tab for the Limelight + val visionTab: ShuffleboardTab = Shuffleboard.getTab("Vision") +>>>>>>> a460a03e86fd31443af08ddf3721adb2fac133d4 } override fun periodic() { + // Update the data with the latest values from the Limelight xOffset = tx.getDouble(0.0) yOffset = ty.getDouble(0.0) area = ta.getDouble(0.0) skew = ts.getDouble(0.0) +<<<<<<< HEAD targetVisible = tv.getDouble(0.0) > 0 +======= + targetVisible = tv.getDouble(0.0) == 1.0 + } + + // Getters for the data from the Limelight + fun getXOffset(): Double { + return xOffset + } + + fun getYOffset(): Double { + return yOffset + } + + fun getArea(): Double { + return area + } + + fun getSkew(): Double { + return skew + } + + fun isTargetVisible(): Boolean { + return targetVisible +>>>>>>> a460a03e86fd31443af08ddf3721adb2fac133d4 } } \ No newline at end of file From 5341a28840c8a6568827df6ff2ca39f74c67064b Mon Sep 17 00:00:00 2001 From: Falon <47285315+falOn-Dev@users.noreply.github.com> Date: Sun, 10 Dec 2023 13:48:40 -0500 Subject: [PATCH 6/6] cleaned up weird merge commit stuff --- .../robot/commands/vision/TrackTargetCommand.kt | 4 ++-- .../frc/robot/subsystems/LimelightSubsystem.kt | 17 ----------------- 2 files changed, 2 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/commands/vision/TrackTargetCommand.kt b/src/main/java/frc/robot/commands/vision/TrackTargetCommand.kt index a9a42317..c11ae584 100644 --- a/src/main/java/frc/robot/commands/vision/TrackTargetCommand.kt +++ b/src/main/java/frc/robot/commands/vision/TrackTargetCommand.kt @@ -29,11 +29,11 @@ class TrackTargetCommand : CommandBase() { override fun initialize() {} override fun execute() { - rotation = pidController.calculate(limelightSubsystem.xOffset, 0.0) + rotation = pidController.calculate(limelightSubsystem.getXOffset(), 0.0) - if((abs(limelightSubsystem.xOffset) < 2 && limelightSubsystem.area < 3.5) && limelightSubsystem.targetVisible){ + if((abs(limelightSubsystem.getXOffset()) < 2 && limelightSubsystem.getArea() < 3.5) && limelightSubsystem.isTargetVisible()){ translation = Translation2d(0.3, -SingletonXboxController.leftX) } else { translation = Translation2d(0.0, -SingletonXboxController.leftX) diff --git a/src/main/java/frc/robot/subsystems/LimelightSubsystem.kt b/src/main/java/frc/robot/subsystems/LimelightSubsystem.kt index ec5cf80a..1916b5ae 100644 --- a/src/main/java/frc/robot/subsystems/LimelightSubsystem.kt +++ b/src/main/java/frc/robot/subsystems/LimelightSubsystem.kt @@ -18,23 +18,13 @@ object LimelightSubsystem : SubsystemBase() { // NetworkTable object for getting data from the Limelight private var table: NetworkTable - -<<<<<<< HEAD - var xOffset: Double = 0.0 - var yOffset: Double = 0.0 - var area: Double = 0.0 - var skew: Double = 0.0 - var targetVisible: Boolean = false - private var visionTab: ShuffleboardTab -======= // Variables for storing data from the Limelight private var xOffset: Double = 0.0 private var yOffset: Double = 0.0 private var area: Double = 0.0 private var skew: Double = 0.0 private var targetVisible: Boolean = false ->>>>>>> a460a03e86fd31443af08ddf3721adb2fac133d4 init { // Get the default NetworkTable for the Limelight @@ -47,7 +37,6 @@ object LimelightSubsystem : SubsystemBase() { tv = table.getEntry("tv") ts = table.getEntry("ts") -<<<<<<< HEAD visionTab = Shuffleboard.getTab("Vision") visionTab.addDouble("X Offset") { xOffset } @@ -56,10 +45,8 @@ object LimelightSubsystem : SubsystemBase() { visionTab.addDouble("Skew") { skew } visionTab.addBoolean("Target Visible") { targetVisible } -======= // Create a Shuffleboard tab for the Limelight val visionTab: ShuffleboardTab = Shuffleboard.getTab("Vision") ->>>>>>> a460a03e86fd31443af08ddf3721adb2fac133d4 } @@ -70,9 +57,6 @@ object LimelightSubsystem : SubsystemBase() { yOffset = ty.getDouble(0.0) area = ta.getDouble(0.0) skew = ts.getDouble(0.0) -<<<<<<< HEAD - targetVisible = tv.getDouble(0.0) > 0 -======= targetVisible = tv.getDouble(0.0) == 1.0 } @@ -95,6 +79,5 @@ object LimelightSubsystem : SubsystemBase() { fun isTargetVisible(): Boolean { return targetVisible ->>>>>>> a460a03e86fd31443af08ddf3721adb2fac133d4 } } \ No newline at end of file