diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index 05a36b82..6a7d21a4 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit 05a36b82516ae4423b67157befc8242b05d57175 +Subproject commit 6a7d21a40a4817213b5d7c4b9bd7adc060814338 diff --git a/src/main/java/competition/operator_interface/OperatorInterface.java b/src/main/java/competition/operator_interface/OperatorInterface.java index 41f6fe57..a90ed534 100644 --- a/src/main/java/competition/operator_interface/OperatorInterface.java +++ b/src/main/java/competition/operator_interface/OperatorInterface.java @@ -107,4 +107,5 @@ public void periodic() { this.operatorGamepadAdvanced.getRumbleManager().periodic(); this.operatorFundamentalsGamepad.getRumbleManager().periodic(); } + } diff --git a/src/main/java/competition/subsystems/lights/LightSubsystem.java b/src/main/java/competition/subsystems/lights/LightSubsystem.java index 095767bd..c6e7b16c 100644 --- a/src/main/java/competition/subsystems/lights/LightSubsystem.java +++ b/src/main/java/competition/subsystems/lights/LightSubsystem.java @@ -1,18 +1,17 @@ - - package competition.subsystems.lights; import javax.inject.Inject; import javax.inject.Singleton; import competition.electrical_contract.ElectricalContract; +import competition.operator_interface.OperatorInterface; 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 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; @@ -21,7 +20,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; @@ -29,20 +28,23 @@ public class LightSubsystem extends BaseSubsystem { final CollectorSubsystem collector; final VisionSubsystem vision; final DynamicOracle oracle; - final XDigitalOutput[] outputs; + + SerialPort serialPort; + private int loopcount = 1; + private final int loopMod = 4; + public boolean lightsWorking = false; boolean ampSignalOn = false; 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 + NoCode(15), DisabledCustomAutoSomeCamerasWorking(13), DisabledCustomAutoNoCamerasWorking(12), DisabledCustomAutoAllCamerasWorking(11), DisabledDefaultAutoSomeCamerasWorking(10), - DisabledDefaultAutoNoCameraWorking(9), + DisabledDefaultAutoNoCameraWorking(2), DisabledDefaultAutoAllCamerasWorking(8), - RobotEnabled(5), + RobotEnabled(34), ShooterReadyWithoutNote(1), ReadyToShoot(2), RobotContainsNote(3), @@ -52,7 +54,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."); } @@ -83,18 +85,19 @@ public LightSubsystem(XDigitalOutputFactory digitalOutputFactory, AutonomousCommandSelector autonomousCommandSelector, ShooterWheelSubsystem shooter, CollectorSubsystem collector, VisionSubsystem vision, - DynamicOracle oracle) { + DynamicOracle oracle, OperatorInterface oi) { 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); - 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; + + // 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); + } } public LightsStateMessage getCurrentState() { @@ -128,31 +131,13 @@ 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: - * 0 -> [false, false, false, false] - * 1 -> [true, false, false, false] - * 14 -> [false, true, true, true] - * 15 -> [true, true, true, true] - */ public static boolean[] convertIntToBits(int value) { boolean[] bits = new boolean[numBits]; for(int i = 0; i < numBits; i++) { @@ -161,16 +146,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); + lightsWorking = true; + 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); + // 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 = String.valueOf(currentState.getValue()); + serialPort.writeString(stateValue + "\n"); + serialPort.flush(); + } catch (Exception e) { + log.info("problem occurred within LightSubsystem " + e.toString()); + } } public void toggleAmpSignal() { ampSignalOn = !ampSignalOn; } - + } 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)); } }