Skip to content
This repository has been archived by the owner on Dec 11, 2024. It is now read-only.

feat(vision): use megatag2 for LL and working LL diagnostics #90

Open
wants to merge 5 commits into
base: main
Choose a base branch
from
Open

feat(vision): use megatag2 for LL and working LL diagnostics #90

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
757e837
feat(vision): use megatag2 for LL and working LL diagnostics
Craftzman7 Oct 7, 2024
34de9f0
chore: cleanup misc code
Craftzman7 Oct 8, 2024
93d6192
feat(auto): add handoff timeout
Craftzman7 Oct 8, 2024
4a04f4e
fix: typo
Craftzman7 Oct 8, 2024
81e41de
fix: band-aid for "unreachable code"
Craftzman7 Oct 9, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 22 additions & 1 deletion src/main/java/com/frcteam3636/frc2024/Dashboard.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,10 +4,31 @@ import com.frcteam3636.frc2024.subsystems.drivetrain.Drivetrain
import com.frcteam3636.frc2024.subsystems.shooter.Shooter
import edu.wpi.first.wpilibj.RobotController
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
import java.net.InetAddress
import kotlin.concurrent.thread

object Dashboard {
private val limelightAddrs = ArrayList<String>()
private var limelightsConnected = true

init {
limelightAddrs.add("10.36.36.11") // TODO: Think of a better way to do this
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do we need the IP of the camera anywhere else?


thread(isDaemon = true) {
while (true) {
var allConnected = true // Prevent the value from flickering
for (addr in limelightAddrs) {
try {
InetAddress.getByName(addr).isReachable(1000)
} catch (err: Exception) {
allConnected = false
break
}
}
limelightsConnected = allConnected
}
}

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do we need to recheck isDamon? Can it be combined or could we just return if false?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

isDaemon is a named parameter to a function and isn't being checked by us - although we could combine these 2 threads into one single "diagnostics" thread

thread(isDaemon = true) {
val canDiagnostics = TalonFXDiagnosticCollector(Drivetrain, Shooter.Pivot)
while (true) {
Expand All @@ -26,6 +47,6 @@ object Dashboard {
fun update() {
SmartDashboard.putBoolean("Battery Full", RobotController.getBatteryVoltage() >= 12.3)
SmartDashboard.putBoolean("NavX OK", Drivetrain.gyroConnected)
SmartDashboard.putBoolean("All Cameras OK", Drivetrain.allCamerasConnected)
SmartDashboard.putBoolean("All Cameras OK", limelightsConnected && Drivetrain.allCamerasConnected)
}
}
25 changes: 8 additions & 17 deletions src/main/java/com/frcteam3636/frc2024/subsystems/drivetrain/AbsolutePose.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@ import edu.wpi.first.math.util.Units
import edu.wpi.first.networktables.NetworkTableInstance
import edu.wpi.first.util.struct.Struct
import edu.wpi.first.util.struct.StructSerializable
import edu.wpi.first.wpilibj.Timer
import org.littletonrobotics.junction.LogTable
import org.littletonrobotics.junction.Logger
import org.littletonrobotics.junction.inputs.LoggableInputs
Expand Down Expand Up @@ -51,22 +50,17 @@ class LimelightPoseIOReal(name: String) : AbsolutePoseIO {
.getTable(name)
private val stddev = VecBuilder.fill(.7, .7, 9999999.0)

private val botPose = table.getDoubleArrayTopic("botpose_wpiblue").subscribe(null)
private val botPose = table.getDoubleArrayTopic("botpose_orb_wpiblue").subscribe(null)
private val cl = table.getDoubleTopic("cl").subscribe(0.0)
private val tl = table.getDoubleTopic("tl").subscribe(0.0)
private val disconnectTimeout = Timer().apply {
start()
}

override fun updateInputs(inputs: AbsolutePoseIO.Inputs) {
// val entries = DoubleArray(6)
// entries[0] = Drivetrain.estimatedPose.rotation.degrees
// entries[1] = 0.0
// entries[2] = 0.0
// entries[3] = 0.0
// entries[4] = 0.0
// entries[5] = 0.0
// table.getEntry("robot_orientation_set").setDoubleArray(entries)
val entries = DoubleArray(6)
for (i in entries.indices) {
entries[i] = 0.0
}
entries[0] = Drivetrain.estimatedPose.rotation.degrees
table.getEntry("robot_orientation_set").setDoubleArray(entries)
inputs.measurement = botPose.readQueue().lastOrNull()?.let { update ->
val x = update.value[0]
val y = update.value[1]
Expand All @@ -76,8 +70,6 @@ class LimelightPoseIOReal(name: String) : AbsolutePoseIO {
val yaw = Units.degreesToRadians(update.value[5])
val tagCount = update.value[7]

disconnectTimeout.restart()

if (tagCount == 0.0 || Drivetrain.gyroRate > 720) {
return
}
Expand All @@ -95,8 +87,7 @@ class LimelightPoseIOReal(name: String) : AbsolutePoseIO {
}
}

override val cameraConnected
get() = disconnectTimeout.hasElapsed(1.0)
override val cameraConnected = true // we determine this value in a ping thread
}

@Suppress("unused")
Expand Down
Loading