[hal] Add systemcore battery reading (#7995)

hal/src/main/native/systemcore/HAL.cpp

@@ -293,6 +293,9 @@ HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) {
     return true;
   }
 
+  // Initialize system server first, other things might use it
+  hal::InitializeSystemServer();
+
   hal::init::InitializeHAL();
 
   hal::init::HAL_IsInitialized.store(true);
@@ -307,8 +310,6 @@ HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) {
     return false;
   }
 
-  hal::InitializeSystemServer();
-
   // // Return false if program failed to kill an existing program
   // if (!killExistingProgram(timeout, mode)) {
   //   return false;

hal/src/main/native/systemcore/Power.cpp

@@ -6,8 +6,12 @@
 
 #include <memory>
 
+#include <networktables/DoubleTopic.h>
+
 #include "HALInitializer.h"
+#include "SystemServerInternal.h"
 #include "hal/Errors.h"
+#include "mrc/NtNetComm.h"
 
 using namespace hal;
 
@@ -19,16 +23,34 @@ static void initializePower(int32_t* status) {
 
 }  // namespace hal
 
+namespace {
+struct SystemServerPower {
+  nt::NetworkTableInstance ntInst;
+
+  nt::DoubleSubscriber batterySubscriber;
+
+  explicit SystemServerPower(nt::NetworkTableInstance inst) {
+    ntInst = inst;
+
+    batterySubscriber =
+        ntInst.GetDoubleTopic(ROBOT_BATTERY_VOLTAGE_PATH).Subscribe(0.0);
+  }
+};
+}  // namespace
+
+static ::SystemServerPower* systemServerPower;
+
 namespace hal::init {
-void InitializePower() {}
+void InitializePower() {
+  systemServerPower = new ::SystemServerPower{hal::GetSystemServer()};
+}
 }  // namespace hal::init
 
 extern "C" {
 
 double HAL_GetVinVoltage(int32_t* status) {
   initializePower(status);
-  *status = HAL_HANDLE_ERROR;
-  return 0;
+  return systemServerPower->batterySubscriber.Get();
 }
 
 double HAL_GetUserVoltage3V3(int32_t* status) {