From e2d2835212d8ec304e453c12bc89bb3c548ee9cd Mon Sep 17 00:00:00 2001 From: Jaden Chen Date: Thu, 7 Mar 2024 21:27:22 -0500 Subject: [PATCH 1/9] initial scanner implementation --- battery_scanner/conda_env.yml | 6 ++++++ battery_scanner/scanner.py | 27 +++++++++++++++++++++++++++ 2 files changed, 33 insertions(+) create mode 100644 battery_scanner/conda_env.yml create mode 100644 battery_scanner/scanner.py diff --git a/battery_scanner/conda_env.yml b/battery_scanner/conda_env.yml new file mode 100644 index 00000000..3d1a477b --- /dev/null +++ b/battery_scanner/conda_env.yml @@ -0,0 +1,6 @@ +channels: + - defaults +dependencies: + - python=3.12 + - pip + - pyserial diff --git a/battery_scanner/scanner.py b/battery_scanner/scanner.py new file mode 100644 index 00000000..a765fb18 --- /dev/null +++ b/battery_scanner/scanner.py @@ -0,0 +1,27 @@ +import serial +import array + +timeout = 5 +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 + +with serial.Serial('COM3', 9600, timeout=timeout) as ser: + + while True: + ser.write(scan_command) + response = ser.read(response_length) + + if len(response) == response_length: + if response.startswith(prefix): + print(f'prefix matches {response.hex()}') + print(f"name: {response[-8:]}") + + else: + print(f'prefix doesnt match {response.hex()}') + print('too short') + + + + From 7169809cb36fb07b0fefdff0743152f670a4054f Mon Sep 17 00:00:00 2001 From: JadenChen1123 Date: Sat, 9 Mar 2024 10:34:08 -0500 Subject: [PATCH 2/9] network table support --- battery_scanner/scanner.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/battery_scanner/scanner.py b/battery_scanner/scanner.py index a765fb18..a5ff7179 100644 --- a/battery_scanner/scanner.py +++ b/battery_scanner/scanner.py @@ -1,11 +1,14 @@ import serial import array +import ntcore timeout = 5 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 +nt_table = ntcore.NetworkTableInstance.getDefault().getTable( + "/" + config_store.local_config.device_id + "/output") with serial.Serial('COM3', 9600, timeout=timeout) as ser: @@ -17,6 +20,7 @@ if response.startswith(prefix): print(f'prefix matches {response.hex()}') print(f"name: {response[-8:]}") + nt_table.append(f"Battery name: {response[-8]}") else: print(f'prefix doesnt match {response.hex()}') From 11e6870ce83ed4587c1838de8be74a71529ae127 Mon Sep 17 00:00:00 2001 From: Jonah <47046556+jwbonner@users.noreply.github.com> Date: Sat, 9 Mar 2024 12:30:51 -0500 Subject: [PATCH 3/9] Add NT battery name interface to robot code --- .../org/littletonrobotics/frc2024/Robot.java | 87 ++++++++++--------- .../frc2024/util/BatteryTracker.java | 84 ------------------ 2 files changed, 48 insertions(+), 123 deletions(-) delete mode 100644 src/main/java/org/littletonrobotics/frc2024/util/BatteryTracker.java diff --git a/src/main/java/org/littletonrobotics/frc2024/Robot.java b/src/main/java/org/littletonrobotics/frc2024/Robot.java index ba09d3e6..9d21abd0 100644 --- a/src/main/java/org/littletonrobotics/frc2024/Robot.java +++ b/src/main/java/org/littletonrobotics/frc2024/Robot.java @@ -11,6 +11,8 @@ import com.ctre.phoenix6.CANBus; import edu.wpi.first.hal.AllianceStationID; +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; @@ -30,7 +32,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; @@ -57,12 +58,19 @@ 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 canErrorTimer = new Timer(); private final Timer canInitialErrorTimer = new Timer(); private final Timer canivoreErrorTimer = new Timer(); + 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 = @@ -82,7 +90,6 @@ public class Robot extends LoggedRobot { 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); @@ -165,30 +172,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); // Instantiate our RobotContainer. This will perform all our button bindings, @@ -274,18 +257,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; - } -} From 115daf5291cda8eb1a2ac769e4d1294e0d1d1c55 Mon Sep 17 00:00:00 2001 From: Leo Huang Date: Sat, 9 Mar 2024 14:53:56 -0500 Subject: [PATCH 4/9] Initial implementation of network tables --- battery_scanner/scanner.py | 57 ++++++++++++++++++++++++++------------ 1 file changed, 39 insertions(+), 18 deletions(-) diff --git a/battery_scanner/scanner.py b/battery_scanner/scanner.py index a5ff7179..478f215f 100644 --- a/battery_scanner/scanner.py +++ b/battery_scanner/scanner.py @@ -1,31 +1,52 @@ +import argparse +from time import sleep import serial import array import ntcore +import logging +import re + +logging.basicConfig(level=logging.DEBUG) + + +parser = argparse.ArgumentParser() +parser.add_argument("--ip", default="192.168.0.1", type=str, help="IP address to connect to") +parser.add_argument("--dev", default="COM8", type=str, help="Path to the camera device") +args = parser.parse_args() timeout = 5 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 -nt_table = ntcore.NetworkTableInstance.getDefault().getTable( - "/" + config_store.local_config.device_id + "/output") +name = None +client = ntcore.NetworkTableInstance.getDefault() +client.startClient4(f'battery-scanner') +client.setServerTeam(6328) -with serial.Serial('COM3', 9600, timeout=timeout) as ser: +pub = client.getStringTopic("/battery_name").publish() + +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) - while True: - ser.write(scan_command) - response = ser.read(response_length) - - if len(response) == response_length: - if response.startswith(prefix): - print(f'prefix matches {response.hex()}') - print(f"name: {response[-8:]}") - nt_table.append(f"Battery name: {response[-8]}") - + if len(response) == response_length: + if response.startswith(prefix): + name_bytes = response[-8:] + name = name_bytes.decode("utf-8") + if re.match('^[a-zA-Z0-9_\\-]+$', name): + logging.info(f"Battery ID: {name}") + break else: - print(f'prefix doesnt match {response.hex()}') - print('too short') - - - + logging.warning("Battery ID doesn't match expected pattern") + +if name is not None: + pub.set(name) +else: + logging.error("Unable to read battery code") +logging.info("Switching to idle mode") +while True: + sleep(100) \ No newline at end of file From 4467a102de01d19082debf260190473ca460c916 Mon Sep 17 00:00:00 2001 From: Leo Huang Date: Sat, 9 Mar 2024 15:01:52 -0500 Subject: [PATCH 5/9] Updated conda env to include pyntcore --- battery_scanner/conda_env.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/battery_scanner/conda_env.yml b/battery_scanner/conda_env.yml index 3d1a477b..f6c272f4 100644 --- a/battery_scanner/conda_env.yml +++ b/battery_scanner/conda_env.yml @@ -4,3 +4,5 @@ dependencies: - python=3.12 - pip - pyserial + - pip: + - pyntcore From 66f45bfdf4b4b301030308f35e661091457a42a6 Mon Sep 17 00:00:00 2001 From: izvit Date: Mon, 11 Mar 2024 03:14:52 -0400 Subject: [PATCH 6/9] Cleaned up code --- battery_scanner/scanner.py | 73 ++++++++++++++++++++++++++------------ 1 file changed, 50 insertions(+), 23 deletions(-) diff --git a/battery_scanner/scanner.py b/battery_scanner/scanner.py index 478f215f..71cbc465 100644 --- a/battery_scanner/scanner.py +++ b/battery_scanner/scanner.py @@ -1,3 +1,5 @@ +""" Script to interface with battery scanner +""" import argparse from time import sleep import serial @@ -6,47 +8,72 @@ import logging import re -logging.basicConfig(level=logging.DEBUG) - - -parser = argparse.ArgumentParser() -parser.add_argument("--ip", default="192.168.0.1", type=str, help="IP address to connect to") -parser.add_argument("--dev", default="COM8", type=str, help="Path to the camera device") -args = parser.parse_args() - +#------------------ +#-- Parameters +#------------------ +logging.basicConfig(level=logging.INFO) timeout = 5 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 -name = None + +#------------------ +#-- 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 Network Tables +#------------------ client = ntcore.NetworkTableInstance.getDefault() client.startClient4(f'battery-scanner') client.setServerTeam(6328) pub = client.getStringTopic("/battery_name").publish() -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 = name_bytes.decode("utf-8") - if re.match('^[a-zA-Z0-9_\\-]+$', name): - logging.info(f"Battery ID: {name}") - break - else: +#------------------ +#-- 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(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 From bc99be9e3fd389c1542467a8d7caa7f9b20ad45b Mon Sep 17 00:00:00 2001 From: izvit Date: Mon, 11 Mar 2024 03:18:19 -0400 Subject: [PATCH 7/9] Added battery name prefix --- battery_scanner/scanner.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/battery_scanner/scanner.py b/battery_scanner/scanner.py index 71cbc465..d5ee729f 100644 --- a/battery_scanner/scanner.py +++ b/battery_scanner/scanner.py @@ -12,6 +12,7 @@ #-- Parameters #------------------ logging.basicConfig(level=logging.INFO) + timeout = 5 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]) @@ -66,7 +67,7 @@ #-- Publish name #------------------ if name is not None: - pub.set(name) + pub.set(f"BAT-{name}") else: logging.error("Unable to read battery code") From 98219b577b1d6563c35abe255e201d92a8cdefcb Mon Sep 17 00:00:00 2001 From: izvit Date: Mon, 11 Mar 2024 03:18:43 -0400 Subject: [PATCH 8/9] Fixed comment --- battery_scanner/scanner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/battery_scanner/scanner.py b/battery_scanner/scanner.py index d5ee729f..e9cb7f80 100644 --- a/battery_scanner/scanner.py +++ b/battery_scanner/scanner.py @@ -28,7 +28,7 @@ #------------------ -#-- Setup Network Tables +#-- Setup NetworkTables #------------------ client = ntcore.NetworkTableInstance.getDefault() client.startClient4(f'battery-scanner') From 8cb54d91476b63cff508521875537b6361cf95e2 Mon Sep 17 00:00:00 2001 From: izvit Date: Mon, 11 Mar 2024 03:19:06 -0400 Subject: [PATCH 9/9] Lowered timeout value --- battery_scanner/scanner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/battery_scanner/scanner.py b/battery_scanner/scanner.py index e9cb7f80..c498f550 100644 --- a/battery_scanner/scanner.py +++ b/battery_scanner/scanner.py @@ -13,7 +13,7 @@ #------------------ logging.basicConfig(level=logging.INFO) -timeout = 5 +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