diff --git a/src/main/java/com/frcteam3636/frc2024/Dashboard.kt b/src/main/java/com/frcteam3636/frc2024/Dashboard.kt index 0c87fef..73dcefc 100644 --- a/src/main/java/com/frcteam3636/frc2024/Dashboard.kt +++ b/src/main/java/com/frcteam3636/frc2024/Dashboard.kt @@ -4,10 +4,32 @@ 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() + private var limelightsConnected = true + init { + limelightAddrs.add("10.36.36.11") // TODO: Think of a better way to do this + + 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 + Thread.sleep(2000) + } + } + thread(isDaemon = true) { val canDiagnostics = TalonFXDiagnosticCollector(Drivetrain, Shooter.Pivot) while (true) { @@ -26,6 +48,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) } } diff --git a/src/main/java/com/frcteam3636/frc2024/Robot.kt b/src/main/java/com/frcteam3636/frc2024/Robot.kt index 70e13ee..bbf86cd 100644 --- a/src/main/java/com/frcteam3636/frc2024/Robot.kt +++ b/src/main/java/com/frcteam3636/frc2024/Robot.kt @@ -118,7 +118,7 @@ object Robot : LoggedRobot() { Commands.sequence( Commands.parallel( Shooter.Pivot.pivotAndStop(Shooter.Pivot.Target.STOWED.profile.position()), - doIntakeSequence() + doIntakeSequence(5.0) ), Commands.parallel( Shooter.Pivot.followMotionProfile(Shooter.Pivot.Target.AIM), @@ -208,7 +208,7 @@ object Robot : LoggedRobot() { controller.rightBumper() .debounce(0.150) .whileTrue( - doIntakeSequence() + doIntakeSequence(null) ) // Outtake @@ -236,7 +236,7 @@ object Robot : LoggedRobot() { Commands.sequence( Commands.waitUntil(Shooter.Flywheels.atDesiredVelocity), Shooter.Feeder.feed().withTimeout(0.4).beforeStarting({ - if (Note.state == Note.State.SHOOTER) { + if (Note.state == Note.State.SHOOTER || Note.state == Note.State.HANDOFF) { Note.state = Note.State.NONE } }), @@ -302,7 +302,7 @@ object Robot : LoggedRobot() { } } -private fun doIntakeSequence(): Command = +private fun doIntakeSequence(handoffTimeout: Double?): Command = Commands.sequence( Intake.intake(), Commands.runOnce({ Note.state = Note.State.HANDOFF }), @@ -324,9 +324,17 @@ private fun doIntakeSequence(): Command = Commands.waitUntil(Trigger { !Shooter.Flywheels.aboveIntakeThreshold }), Commands.runOnce({ Note.state = Note.State.SHOOTER }) ) +// .let { +// if (handoffTimeout != null) { +// return it.withTimeout(handoffTimeout) +// } else { +// return it +// } +// } ) ) + object Note { enum class State(val index: Long) { NONE(0), diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/drivetrain/AbsolutePose.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/drivetrain/AbsolutePose.kt index faeb33a..66fe2a0 100644 --- a/src/main/java/com/frcteam3636/frc2024/subsystems/drivetrain/AbsolutePose.kt +++ b/src/main/java/com/frcteam3636/frc2024/subsystems/drivetrain/AbsolutePose.kt @@ -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 @@ -51,22 +50,14 @@ 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) { 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] @@ -76,8 +67,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 } @@ -95,8 +84,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")