diff --git a/battery_scanner/conda_env.yml b/battery_scanner/conda_env.yml new file mode 100644 index 00000000..f6c272f4 --- /dev/null +++ b/battery_scanner/conda_env.yml @@ -0,0 +1,8 @@ +channels: + - defaults +dependencies: + - python=3.12 + - pip + - pyserial + - pip: + - pyntcore diff --git a/battery_scanner/scanner.py b/battery_scanner/scanner.py new file mode 100644 index 00000000..c498f550 --- /dev/null +++ b/battery_scanner/scanner.py @@ -0,0 +1,80 @@ +""" Script to interface with battery scanner +""" +import argparse +from time import sleep +import serial +import array +import ntcore +import logging +import re + +#------------------ +#-- Parameters +#------------------ +logging.basicConfig(level=logging.INFO) + +timeout = 1 +prefix = array.array('B', [0x02, 0x00, 0x00, 0x01, 0x00, 0x33, 0x31]) +scan_command = array.array('B',[0x7e, 0x00, 0x08, 0x01, 0x00, 0x02, 0x01, 0xab, 0xcd]) +name_length = 8 +response_length = len(prefix) + name_length + +#------------------ +#-- Parse command line arguments +#------------------ +parser = argparse.ArgumentParser() +parser.add_argument("--dev", default="COM8", type=str, help="Path to the camera device") +args = parser.parse_args() + + +#------------------ +#-- Setup NetworkTables +#------------------ +client = ntcore.NetworkTableInstance.getDefault() +client.startClient4(f'battery-scanner') +client.setServerTeam(6328) + +pub = client.getStringTopic("/battery_name").publish() + +#------------------ +#-- Read battery name +#------------------ +name = None +try: + with serial.Serial(args.dev, 9600, timeout=timeout) as ser: + + logging.info("Scanning for battery ID") + + while True: + ser.write(scan_command) + response = ser.read(response_length) + + if len(response) == response_length: + if response.startswith(prefix): + name_bytes = response[-8:] + name_str = name_bytes.decode("utf-8") + if re.match('^[a-zA-Z0-9_\\-]+$', name_str): + name = name_str + logging.info(f"Battery ID: {name}") + break + else: + logging.warning("Battery ID doesn't match expected pattern") + +except Exception as e: + logging.error(e) + +#------------------ +#-- Publish name +#------------------ +if name is not None: + pub.set(f"BAT-{name}") +else: + logging.error("Unable to read battery code") + + +#------------------ +#-- Switch to idle state +#------------------ +logging.info("Switching to idle mode") +while True: + sleep(100) \ No newline at end of file diff --git a/src/main/java/org/littletonrobotics/frc2024/Robot.java b/src/main/java/org/littletonrobotics/frc2024/Robot.java index f7465454..0122bc74 100644 --- a/src/main/java/org/littletonrobotics/frc2024/Robot.java +++ b/src/main/java/org/littletonrobotics/frc2024/Robot.java @@ -10,6 +10,8 @@ import static org.littletonrobotics.frc2024.util.Alert.AlertType; import com.ctre.phoenix6.CANBus; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StringSubscriber; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.Threads; @@ -29,7 +31,6 @@ import org.littletonrobotics.frc2024.Constants.Mode; import org.littletonrobotics.frc2024.subsystems.leds.Leds; import org.littletonrobotics.frc2024.util.Alert; -import org.littletonrobotics.frc2024.util.BatteryTracker; import org.littletonrobotics.frc2024.util.NoteVisualizer; import org.littletonrobotics.frc2024.util.VirtualSubsystem; import org.littletonrobotics.junction.LogFileUtil; @@ -56,7 +57,6 @@ public class Robot extends LoggedRobot { private RobotContainer robotContainer; private double autoStart; private boolean autoMessagePrinted; - private boolean batteryNameWritten = false; private final Timer disabledTimer = new Timer(); private final Timer canInitialErrorTimer = new Timer(); private final Timer canErrorTimer = new Timer(); @@ -64,6 +64,14 @@ public class Robot extends LoggedRobot { private double teleStart; private static double teleElapsedTime = 0.0; + private static final String defaultBatteryName = "BAT-0000-000"; + private final StringSubscriber batteryNameSubscriber = + NetworkTableInstance.getDefault() + .getStringTopic("/battery_name") + .subscribe(defaultBatteryName); + private boolean batteryNameChecked = false; + private boolean batteryNameWritten = false; + private final Alert canErrorAlert = new Alert("CAN errors detected, robot may not be controllable.", AlertType.ERROR); private final Alert canivoreErrorAlert = @@ -93,7 +101,6 @@ public void robotInit() { // Record metadata Logger.recordMetadata("Robot", Constants.getRobot().toString()); - Logger.recordMetadata("BatteryName", "BAT-" + BatteryTracker.scanBattery(1.5)); Logger.recordMetadata("TuningMode", Boolean.toString(Constants.tuningMode)); Logger.recordMetadata("RuntimeType", getRuntimeType().toString()); Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); @@ -171,30 +178,6 @@ public void robotInit() { canivoreErrorTimer.restart(); disabledTimer.restart(); - // Check for battery alert - if (Constants.getMode() == Mode.REAL - && !BatteryTracker.getName().equals(BatteryTracker.defaultName)) { - File file = new File(batteryNameFile); - if (file.exists()) { - // Read previous battery name - String previousBatteryName = ""; - try { - previousBatteryName = - new String(Files.readAllBytes(Paths.get(batteryNameFile)), StandardCharsets.UTF_8); - } catch (IOException e) { - e.printStackTrace(); - } - if (previousBatteryName.equals(BatteryTracker.getName())) { - // Same battery, set alert - sameBatteryAlert.set(true); - Leds.getInstance().sameBattery = true; - } else { - // New battery, delete file - file.delete(); - } - } - } - RobotController.setBrownoutVoltage(6.0); robotContainer = new RobotContainer(); } @@ -278,18 +261,44 @@ public void robotPeriodic() { Leds.getInstance().lowBatteryAlert = true; } - // Write battery name if connected to field - if (Constants.getMode() == Mode.REAL - && !batteryNameWritten - && !BatteryTracker.getName().equals(BatteryTracker.defaultName) - && DriverStation.isFMSAttached()) { - batteryNameWritten = true; - try { - FileWriter fileWriter = new FileWriter(batteryNameFile); - fileWriter.write(BatteryTracker.getName()); - fileWriter.close(); - } catch (IOException e) { - e.printStackTrace(); + // Update battery alert + String batteryName = batteryNameSubscriber.get(); + Logger.recordOutput("BatteryName", batteryName); + if (Constants.getMode() == Mode.REAL && !batteryName.equals(defaultBatteryName)) { + // Check for battery alert + if (!batteryNameChecked) { + batteryNameChecked = true; + File file = new File(batteryNameFile); + if (file.exists()) { + // Read previous battery name + String previousBatteryName = ""; + try { + previousBatteryName = + new String(Files.readAllBytes(Paths.get(batteryNameFile)), StandardCharsets.UTF_8); + } catch (IOException e) { + e.printStackTrace(); + } + if (previousBatteryName.equals(batteryName)) { + // Same battery, set alert + sameBatteryAlert.set(true); + Leds.getInstance().sameBattery = true; + } else { + // New battery, delete file + file.delete(); + } + } + } + + // Write battery name if connected to FMS + if (!batteryNameWritten && DriverStation.isFMSAttached()) { + batteryNameWritten = true; + try { + FileWriter fileWriter = new FileWriter(batteryNameFile); + fileWriter.write(batteryName); + fileWriter.close(); + } catch (IOException e) { + e.printStackTrace(); + } } } diff --git a/src/main/java/org/littletonrobotics/frc2024/util/BatteryTracker.java b/src/main/java/org/littletonrobotics/frc2024/util/BatteryTracker.java deleted file mode 100644 index e779e58c..00000000 --- a/src/main/java/org/littletonrobotics/frc2024/util/BatteryTracker.java +++ /dev/null @@ -1,84 +0,0 @@ -// Copyright (c) 2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file at -// the root directory of this project. - -package org.littletonrobotics.frc2024.util; - -import edu.wpi.first.wpilibj.SerialPort; -import java.util.Arrays; -import java.util.List; -import lombok.Getter; -import org.littletonrobotics.frc2024.Constants; -import org.littletonrobotics.frc2024.Constants.Mode; -import org.littletonrobotics.frc2024.Constants.RobotType; - -public class BatteryTracker { - private static final List supportedRobots = List.of(RobotType.COMPBOT); - public static final String defaultName = "0000-000"; - - private static final int nameLength = 8; - private static final byte[] scanCommand = - new byte[] {0x7e, 0x00, 0x08, 0x01, 0x00, 0x02, 0x01, (byte) 0xab, (byte) 0xcd}; - private static final byte[] responsePrefix = - new byte[] {0x02, 0x00, 0x00, 0x01, 0x00, 0x33, 0x31}; - private static final int fullResponseLength = responsePrefix.length + nameLength; - - /** -- GETTER -- Returns the name of the last scanned battery. */ - @Getter private static String name = defaultName; - - /** - * Scans the battery. This should be called before the first loop cycle - * - * @param timeout The time to wait before giving up - */ - public static String scanBattery(double timeout) { - if (Constants.getMode() == Mode.REAL) { - if (supportedRobots.contains(Constants.getRobot())) { - // Only scan on supported robots and in real mode - - try (SerialPort port = new SerialPort(9600, SerialPort.Port.kUSB)) { - port.setTimeout(timeout); - port.setWriteBufferSize(scanCommand.length); - port.setReadBufferSize(fullResponseLength); - - port.write(scanCommand, scanCommand.length); - byte[] response = port.read(fullResponseLength); - - // Ensure response is correct length - if (response.length != fullResponseLength) { - System.out.println( - "[BatteryTracker] Expected " - + fullResponseLength - + " bytes from scanner, got " - + response.length); - return name; - } - - // Ensure response starts with prefix - for (int i = 0; i < responsePrefix.length; i++) { - if (response[i] != responsePrefix[i]) { - System.out.println("[BatteryTracker] Invalid prefix from scanner. Got data:"); - System.out.println("[BatteryTracker] " + Arrays.toString(response)); - return name; - } - } - - // Read name from data - byte[] batteryNameBytes = new byte[nameLength]; - System.arraycopy(response, responsePrefix.length, batteryNameBytes, 0, nameLength); - name = new String(batteryNameBytes); - System.out.println("[BatteryTracker] Scanned battery " + name); - - } catch (Exception e) { - System.out.println("[BatteryTracker] Exception while trying to scan battery"); - e.printStackTrace(); - } - } - } - - return name; - } -}