From 8f6278da77a8e343c823aad07a5b9579ea4d9627 Mon Sep 17 00:00:00 2001 From: j0ndough Date: Sat, 10 Aug 2024 13:39:30 -0700 Subject: [PATCH 1/7] revert behavior to use serial as comms --- .../subsystems/lights/LightSubsystem.java | 63 +++++++++++++++++-- 1 file changed, 57 insertions(+), 6 deletions(-) diff --git a/src/main/java/competition/subsystems/lights/LightSubsystem.java b/src/main/java/competition/subsystems/lights/LightSubsystem.java index 095767bd..de1d202a 100644 --- a/src/main/java/competition/subsystems/lights/LightSubsystem.java +++ b/src/main/java/competition/subsystems/lights/LightSubsystem.java @@ -11,11 +11,13 @@ import competition.subsystems.shooter.ShooterWheelSubsystem; import competition.subsystems.vision.VisionSubsystem; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.SerialPort; 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.io.Serial; import java.util.Objects; @Singleton @@ -29,7 +31,12 @@ public class LightSubsystem extends BaseSubsystem { final CollectorSubsystem collector; final VisionSubsystem vision; final DynamicOracle oracle; - final XDigitalOutput[] outputs; + //final XDigitalOutput[] outputs; + + SerialPort serialPort; + private int loopcount = 1; + private final int loopMod = 4; + public boolean lightsWorking = false; boolean ampSignalOn = false; @@ -52,7 +59,7 @@ public enum LightsStateMessage{ LightsStateMessage(final int value) { if(value > maxValue || value < 0) { // it should be okay to have this throw because it will happen immediately on robot startup - // so we'll see failures here in CI before deploying to the robot. + // so we'll see failures here in CI before deploying to the robot. // Getting the RobotAssertionManager in here was proving tricky System.out.println("Values must be between 0 and " + maxValue + " inclusive. Got " + value + " instead. Will always return 0 for safety."); } @@ -89,11 +96,23 @@ public LightSubsystem(XDigitalOutputFactory digitalOutputFactory, this.shooter = shooter; this.vision = vision; this.oracle = oracle; + + // serial test + if (usbIsNotConnected(SerialPort.Port.kUSB1)) { + if (usbIsNotConnected(SerialPort.Port.kUSB2)) { + if (usbIsNotConnected(SerialPort.Port.kUSB)) { + log.error("Lights - could not find a USB port."); + } + } + } + serialPort.setTimeout(0.05); + /* this.outputs = new XDigitalOutput[numBits]; this.outputs[0] = digitalOutputFactory.create(contract.getLightsDio0().channel); this.outputs[1] = digitalOutputFactory.create(contract.getLightsDio1().channel); this.outputs[2] = digitalOutputFactory.create(contract.getLightsDio2().channel); this.outputs[3] = digitalOutputFactory.create(contract.getLightsDio3().channel); + */ //this.pf = pf; } @@ -128,26 +147,28 @@ public LightsStateMessage getCurrentState() { } else if (vision.checkIfSideCamsSeeNote()) { currentState = LightsStateMessage.SideCamsSeeNotes; - }else { + } else { currentState = LightsStateMessage.RobotEnabled; } } return currentState; } + /* public void sendState(LightsStateMessage state) { var bits = convertIntToBits(state.getValue()); for(int i = 0; i < numBits; i++) { outputs[i].set(bits[i]); } } + */ /** * Convert an integer to a boolean array representing the bits of the integer. * The leftmost bit in the result is the least significant bit of the integer. * This was chosen so we could add new bits onto the end of the array easily without changing * how earlier numbers were represented. - * Eg: + * Eg: * 0 -> [false, false, false, false] * 1 -> [true, false, false, false] * 14 -> [false, true, true, true] @@ -161,16 +182,46 @@ public static boolean[] convertIntToBits(int value) { return bits; } + public boolean usbIsNotConnected(SerialPort.Port port) { + try { + serialPort = new SerialPort(9600, port, 8); + serialPort.setWriteBufferMode(SerialPort.WriteBufferMode.kFlushOnAccess); + return false; + } catch (Exception e) { + log.error("Lights are not working: %s", e); + return true; + } + } + @Override public void periodic() { var currentState = getCurrentState(); aKitLog.record("LightState", currentState.toString()); - sendState(currentState); + // sendState(currentState); + + // try sending over serial + if (!lightsWorking) { + return; + } + try { + serialPort.reset(); + // run every 1/10th of a second + if (this.loopcount++ % loopMod != 0) { + return; + } + + // write serial data to lights + String stateValue = currentState.toString(); + serialPort.writeString(stateValue + "\n"); + serialPort.flush(); + } catch (Exception e) { + log.info("problem occurred within LightSubsystem " + e.toString()); + } } public void toggleAmpSignal() { ampSignalOn = !ampSignalOn; } - + } From b2eaa43e1ab826318c7737c8946204dd8d974b45 Mon Sep 17 00:00:00 2001 From: j0ndough Date: Sat, 24 Aug 2024 14:00:13 -0700 Subject: [PATCH 2/7] add detection for USB log drive --- SeriouslyCommonLib | 2 +- .../competition/subsystems/lights/LightSubsystem.java | 8 ++------ 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index aeb273f1..96d04643 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit aeb273f1dff11b2a95b051e3d06ee926e42cdd88 +Subproject commit 96d04643dbaada7c05bf33dcd132e2e588ccd41b diff --git a/src/main/java/competition/subsystems/lights/LightSubsystem.java b/src/main/java/competition/subsystems/lights/LightSubsystem.java index de1d202a..0cea61ec 100644 --- a/src/main/java/competition/subsystems/lights/LightSubsystem.java +++ b/src/main/java/competition/subsystems/lights/LightSubsystem.java @@ -98,12 +98,8 @@ public LightSubsystem(XDigitalOutputFactory digitalOutputFactory, this.oracle = oracle; // serial test - if (usbIsNotConnected(SerialPort.Port.kUSB1)) { - if (usbIsNotConnected(SerialPort.Port.kUSB2)) { - if (usbIsNotConnected(SerialPort.Port.kUSB)) { - log.error("Lights - could not find a USB port."); - } - } + if (usbIsNotConnected(SerialPort.Port.kUSB1)) { // Top port should map to kUSB1. Bottom port is for USB drive + log.error("Lights - could not find a valid USB serial port."); } serialPort.setTimeout(0.05); /* From 01559e5481e369ad51c8f4b22b4ca4f1b52260a0 Mon Sep 17 00:00:00 2001 From: j0ndough Date: Tue, 17 Sep 2024 11:39:11 -0700 Subject: [PATCH 3/7] clean up code, working implementation of USB over serial, needs testing with another USB drive --- SeriouslyCommonLib | 2 +- .../subsystems/lights/LightSubsystem.java | 21 +++++++++++-------- .../subsystems/lights/LightSubsystemTest.java | 6 +++--- 3 files changed, 16 insertions(+), 13 deletions(-) diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index 96d04643..d2dbb89c 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit 96d04643dbaada7c05bf33dcd132e2e588ccd41b +Subproject commit d2dbb89cf5ec6699395a7a1bddd5101384a599c5 diff --git a/src/main/java/competition/subsystems/lights/LightSubsystem.java b/src/main/java/competition/subsystems/lights/LightSubsystem.java index 0cea61ec..8e4abe3f 100644 --- a/src/main/java/competition/subsystems/lights/LightSubsystem.java +++ b/src/main/java/competition/subsystems/lights/LightSubsystem.java @@ -23,7 +23,7 @@ @Singleton public class LightSubsystem extends BaseSubsystem { // based on the number of bits we have, this is the highest number we can send - static final int numBits = 4; + static final int numBits = 6; static final int maxValue = (int)(Math.pow(2, numBits) - 1); final AutonomousCommandSelector autonomousCommandSelector; @@ -47,9 +47,9 @@ public enum LightsStateMessage{ DisabledCustomAutoNoCamerasWorking(12), DisabledCustomAutoAllCamerasWorking(11), DisabledDefaultAutoSomeCamerasWorking(10), - DisabledDefaultAutoNoCameraWorking(9), + DisabledDefaultAutoNoCameraWorking(2), DisabledDefaultAutoAllCamerasWorking(8), - RobotEnabled(5), + RobotEnabled(34), ShooterReadyWithoutNote(1), ReadyToShoot(2), RobotContainsNote(3), @@ -97,11 +97,12 @@ public LightSubsystem(XDigitalOutputFactory digitalOutputFactory, this.vision = vision; this.oracle = oracle; - // serial test + // Connect to USB port over serial if (usbIsNotConnected(SerialPort.Port.kUSB1)) { // Top port should map to kUSB1. Bottom port is for USB drive log.error("Lights - could not find a valid USB serial port."); + } else { // when correct SerialPort is found + serialPort.setTimeout(0.05); } - serialPort.setTimeout(0.05); /* this.outputs = new XDigitalOutput[numBits]; this.outputs[0] = digitalOutputFactory.create(contract.getLightsDio0().channel); @@ -157,7 +158,7 @@ public void sendState(LightsStateMessage state) { outputs[i].set(bits[i]); } } - */ + */ /** * Convert an integer to a boolean array representing the bits of the integer. @@ -182,6 +183,7 @@ public boolean usbIsNotConnected(SerialPort.Port port) { try { serialPort = new SerialPort(9600, port, 8); serialPort.setWriteBufferMode(SerialPort.WriteBufferMode.kFlushOnAccess); + lightsWorking = true; return false; } catch (Exception e) { log.error("Lights are not working: %s", e); @@ -193,7 +195,6 @@ public boolean usbIsNotConnected(SerialPort.Port port) { public void periodic() { var currentState = getCurrentState(); aKitLog.record("LightState", currentState.toString()); - // sendState(currentState); // try sending over serial if (!lightsWorking) { @@ -208,8 +209,10 @@ public void periodic() { } // write serial data to lights - String stateValue = currentState.toString(); - serialPort.writeString(stateValue + "\n"); + String stateValue = String.valueOf(currentState.getValue()); + System.out.println("current stateValue: " + stateValue); + System.out.println(serialPort.writeString(stateValue + "\n")); // debug print + //serialPort.writeString(stateValue + "\n"); serialPort.flush(); } catch (Exception e) { log.info("problem occurred within LightSubsystem " + e.toString()); diff --git a/src/test/java/competition/subsystems/lights/LightSubsystemTest.java b/src/test/java/competition/subsystems/lights/LightSubsystemTest.java index 280b1324..d2dd0159 100644 --- a/src/test/java/competition/subsystems/lights/LightSubsystemTest.java +++ b/src/test/java/competition/subsystems/lights/LightSubsystemTest.java @@ -16,8 +16,8 @@ public static void assertArraysEqual(boolean[] expected, boolean[] actual) { @Test public void testStateToBitsMapping() { - assertArraysEqual(new boolean[] {false, false, false, false}, LightSubsystem.convertIntToBits(0)); - assertArraysEqual(new boolean[] {true, false, false, false}, LightSubsystem.convertIntToBits(1)); - assertArraysEqual(new boolean[] {true, true, true, true}, LightSubsystem.convertIntToBits(15)); + assertArraysEqual(new boolean[] {false, false, false, false, false, false}, LightSubsystem.convertIntToBits(0)); + assertArraysEqual(new boolean[] {true, false, false, false, false, false}, LightSubsystem.convertIntToBits(1)); + assertArraysEqual(new boolean[] {true, true, true, true, false, false}, LightSubsystem.convertIntToBits(15)); } } From 3b270bc68bb2f4a2c3016b893dc5aca18deddcfc Mon Sep 17 00:00:00 2001 From: j0ndough Date: Sat, 14 Dec 2024 23:38:09 -0800 Subject: [PATCH 4/7] push submodule --- SeriouslyCommonLib | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index d2dbb89c..4ba1f477 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit d2dbb89cf5ec6699395a7a1bddd5101384a599c5 +Subproject commit 4ba1f477ec14389fb3e4afb310a648cb8efc4104 From 408b4d706c8419cad492bf2bd1f1b3ea994a8839 Mon Sep 17 00:00:00 2001 From: j0ndough Date: Sat, 14 Dec 2024 23:49:30 -0800 Subject: [PATCH 5/7] fix branching issues --- SeriouslyCommonLib | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index 4ba1f477..fdf446ed 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit 4ba1f477ec14389fb3e4afb310a648cb8efc4104 +Subproject commit fdf446ed61b6be4af9a473d3a6492fd7a0cd4047 From 2a79bfd67306904570b3b42505bb42da0a055cc0 Mon Sep 17 00:00:00 2001 From: j0ndough Date: Sat, 14 Dec 2024 23:52:26 -0800 Subject: [PATCH 6/7] fix branching issues --- SeriouslyCommonLib | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index fdf446ed..6a7d21a4 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit fdf446ed61b6be4af9a473d3a6492fd7a0cd4047 +Subproject commit 6a7d21a40a4817213b5d7c4b9bd7adc060814338 From 0387fc2b8b5492d74b06ad6b398aef7f42422a33 Mon Sep 17 00:00:00 2001 From: j0ndough Date: Sun, 15 Dec 2024 00:06:15 -0800 Subject: [PATCH 7/7] remove debugging info --- .../competition/operator_interface/OperatorInterface.java | 3 --- .../java/competition/subsystems/lights/LightSubsystem.java | 7 ------- 2 files changed, 10 deletions(-) diff --git a/src/main/java/competition/operator_interface/OperatorInterface.java b/src/main/java/competition/operator_interface/OperatorInterface.java index 7dca290e..a90ed534 100644 --- a/src/main/java/competition/operator_interface/OperatorInterface.java +++ b/src/main/java/competition/operator_interface/OperatorInterface.java @@ -108,7 +108,4 @@ public void periodic() { this.operatorFundamentalsGamepad.getRumbleManager().periodic(); } - public double quickCheck() { - return driverGamepad.getLeftStickX(); - } } diff --git a/src/main/java/competition/subsystems/lights/LightSubsystem.java b/src/main/java/competition/subsystems/lights/LightSubsystem.java index 98c91c05..c6e7b16c 100644 --- a/src/main/java/competition/subsystems/lights/LightSubsystem.java +++ b/src/main/java/competition/subsystems/lights/LightSubsystem.java @@ -28,8 +28,6 @@ public class LightSubsystem extends BaseSubsystem { final CollectorSubsystem collector; final VisionSubsystem vision; final DynamicOracle oracle; - final OperatorInterface oi; - //final XDigitalOutput[] outputs; SerialPort serialPort; private int loopcount = 1; @@ -93,7 +91,6 @@ public LightSubsystem(XDigitalOutputFactory digitalOutputFactory, this.shooter = shooter; this.vision = vision; this.oracle = oracle; - this.oi = oi; // Connect to USB port over serial if (usbIsNotConnected(SerialPort.Port.kUSB1)) { // Top port should map to kUSB1. Bottom port is for USB drive @@ -134,8 +131,6 @@ public LightsStateMessage getCurrentState() { } else if (vision.checkIfSideCamsSeeNote()) { currentState = LightsStateMessage.SideCamsSeeNotes; - } else if (oi.quickCheck() >= 0.1) { - currentState = LightsStateMessage.ShooterReadyWithoutNote; } else { currentState = LightsStateMessage.RobotEnabled; } @@ -168,8 +163,6 @@ public void periodic() { var currentState = getCurrentState(); aKitLog.record("LightState", currentState.toString()); - System.out.println(oi.quickCheck()); - // Try sending over serial if (!lightsWorking) { return;