Add battery scanner (#187)

* initial scanner implementation * network table support * Add NT battery name interface to robot code * Initial implementation of network tables * Updated conda env to include pyntcore * Cleaned up code * Added battery name prefix * Fixed comment * Lowered timeout value --------- Co-authored-by: Jaden Chen <[email protected]> Co-authored-by: JadenChen1123 <[email protected]> Co-authored-by: Jonah <[email protected]> Co-authored-by: Leo Huang <[email protected]> Co-authored-by: izvit <[email protected]>
Mechanical-Advantage · Mar 14, 2024 · 0ce425e · 0ce425e
1 parent 180af7a
commit 0ce425e
Show file tree

Hide file tree

Showing 4 changed files with 136 additions and 123 deletions.
diff --git a/battery_scanner/conda_env.yml 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
@@ -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)
diff --git 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,14 +57,21 @@ 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();
   private final Timer canivoreErrorTimer = new Timer();
   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