re-implement serial lights

src/main/java/competition/operator_interface/OperatorInterface.java

@@ -107,4 +107,5 @@ public void periodic() {
         this.operatorGamepadAdvanced.getRumbleManager().periodic();
         this.operatorFundamentalsGamepad.getRumbleManager().periodic();
     }
+
 }

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,28 +20,31 @@
 @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;
     final ShooterWheelSubsystem shooter;
     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;
     }
-   
+
 }

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));
     }
 }