From 01d9a662889712d92afbab22427dcbd6c7f9da02 Mon Sep 17 00:00:00 2001 From: Emily <55307528+rzhai2@users.noreply.github.com> Date: Sat, 30 Mar 2024 13:04:53 -0700 Subject: [PATCH] =?UTF-8?q?Add=20lights=20on=20java=20side=20to=20tell=20d?= =?UTF-8?q?ifference=20between=20number=20of=20cameras=20=E2=80=A6=20(#273?= =?UTF-8?q?)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Add lights on java side to tell difference between number of cameras working * change the dirty flag design to compare command name cuz the dirty flag design might not reflect changing to custom and change back to default * switch status code * address PR comments * fix the logic bug on light states * take out the exception * fix the logical error that causes incorrect info --- src/main/java/competition/Robot.java | 1 - .../subsystems/lights/LightSubsystem.java | 41 ++++++++++++------- .../subsystems/vision/VisionSubsystem.java | 13 ++++++ 3 files changed, 40 insertions(+), 15 deletions(-) diff --git a/src/main/java/competition/Robot.java b/src/main/java/competition/Robot.java index 900e13c8..c2134985 100644 --- a/src/main/java/competition/Robot.java +++ b/src/main/java/competition/Robot.java @@ -59,7 +59,6 @@ protected void initializeSystems() { var autoSelector = getInjectorComponent().autonomousCommandSelector(); autoSelector.setCurrentAutonomousCommand(defaultAuto); - autoSelector.setIsDefault(true); LogTable.disableProtobufWarning(); } diff --git a/src/main/java/competition/subsystems/lights/LightSubsystem.java b/src/main/java/competition/subsystems/lights/LightSubsystem.java index d6848f41..0fe3ca1f 100644 --- a/src/main/java/competition/subsystems/lights/LightSubsystem.java +++ b/src/main/java/competition/subsystems/lights/LightSubsystem.java @@ -9,12 +9,15 @@ import competition.subsystems.collector.CollectorSubsystem; import competition.subsystems.oracle.DynamicOracle; import competition.subsystems.shooter.ShooterWheelSubsystem; +import competition.subsystems.vision.VisionSubsystem; import edu.wpi.first.wpilibj.DriverStation; import xbot.common.command.BaseSubsystem; import xbot.common.controls.actuators.XDigitalOutput; import xbot.common.controls.actuators.XDigitalOutput.XDigitalOutputFactory; import xbot.common.subsystems.autonomous.AutonomousCommandSelector; +import java.util.Objects; + @Singleton public class LightSubsystem extends BaseSubsystem { // based on the number of bits we have, this is the highest number we can send @@ -24,6 +27,7 @@ public class LightSubsystem extends BaseSubsystem { final AutonomousCommandSelector autonomousCommandSelector; final ShooterWheelSubsystem shooter; final CollectorSubsystem collector; + final VisionSubsystem vision; final DynamicOracle oracle; final XDigitalOutput[] outputs; @@ -32,8 +36,12 @@ public class LightSubsystem extends BaseSubsystem { public enum LightsStateMessage{ NoCode(15), // we never send this one, it's implicit when the robot is off // and all of the DIOs float high - WithDefaultAuto(7), - WithCustomAuto(6), + DisabledCustomAutoSomeCamerasWorking(13), + DisabledCustomAutoNoCamerasWorking(12), + DisabledCustomAutoAllCamerasWorking(11), + DisabledDefaultAutoSomeCamerasWorking(10), + DisabledDefaultAutoNoCameraWorking(9), + DisabledDefaultAutoAllCamerasWorking(8), RobotEnabled(5), ShooterReadyWithoutNote(1), ReadyToShoot(2), @@ -57,6 +65,15 @@ public int getValue() { } return value; } + + public static LightsStateMessage getStringValueFromInt(int i) { + for (LightsStateMessage states : LightsStateMessage.values()) { + if (states.getValue() == i) { + return states; + } + } + return LightsStateMessage.NoCode; + } } @Inject @@ -64,10 +81,12 @@ public LightSubsystem(XDigitalOutputFactory digitalOutputFactory, ElectricalContract contract, AutonomousCommandSelector autonomousCommandSelector, ShooterWheelSubsystem shooter, CollectorSubsystem collector, + VisionSubsystem vision, DynamicOracle oracle) { this.autonomousCommandSelector = autonomousCommandSelector; this.collector = collector; this.shooter = shooter; + this.vision = vision; this.oracle = oracle; this.outputs = new XDigitalOutput[numBits]; this.outputs[0] = digitalOutputFactory.create(contract.getLightsDio0().channel); @@ -85,19 +104,14 @@ public LightsStateMessage getCurrentState() { // Not sure about if the way we are checking the shooter is correct (and collector) if (!dsEnabled) { // Check if auto program is set - //isDefault = pf.createPersistentProperty("IsDefaultAuto", autonomousCommandSelector.getIsDefault()?1.0:2.0); - if (autonomousCommandSelector.getIsDefault()) { - currentState = LightsStateMessage.WithDefaultAuto; - } else { - currentState = LightsStateMessage.WithCustomAuto; + int base = 8; + if (!Objects.equals(autonomousCommandSelector.getProgramName(), "SubwooferShotFromMidShootThenShootNearestThree")) { + // Not default + base = 11; } - + // 0 as no camera working, 1 as all camera working, 2 as some camera working + currentState = LightsStateMessage.getStringValueFromInt(base + vision.cameraWorkingState()); } else { - // Try and match enabled states - //if (ampSignalOn) { - //currentState = LightsStateMessage.AmpSignal; - - //} else if (shooter.isReadyToFire() && collector.checkSensorForLights()) { currentState = LightsStateMessage.ReadyToShoot; @@ -114,7 +128,6 @@ public LightsStateMessage getCurrentState() { currentState = LightsStateMessage.RobotEnabled; } } - return currentState; } diff --git a/src/main/java/competition/subsystems/vision/VisionSubsystem.java b/src/main/java/competition/subsystems/vision/VisionSubsystem.java index 6e502a2f..d63ed470 100644 --- a/src/main/java/competition/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/competition/subsystems/vision/VisionSubsystem.java @@ -462,4 +462,17 @@ public void refreshDataFrame() { } centerlineNoteCamera.refreshDataFrame(); } + + public int cameraWorkingState() { + if (allCameras.stream().allMatch(state -> state.isCameraWorking())) { + // If all are working, return 0 + return 0; + } + else if (allCameras.stream().allMatch(state -> !state.isCameraWorking())) { + // If no cameras are working, return 1 + return 1; + } + // If some of the cameras are working, return 2 + return 2; + } }