Merge pull request #9 from Team2537/6-add-limelight-support

6 add limelight support

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)
     }
 }

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.getXOffset(), 0.0)
+
+
+
+        if((abs(limelightSubsystem.getXOffset()) < 2 && limelightSubsystem.getArea() < 3.5) && limelightSubsystem.isTargetVisible()){
+            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) {}
+}

src/main/java/frc/robot/subsystems/LimelightSubsystem.kt

@@ -0,0 +1,83 @@
+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() {
+
+    // 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
+    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
+
+    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")
+
+        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 }
+
+        // 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.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
+    }
+}

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)
     }
 
-
-
 }