Deduplicate code for sending out RobotState messages
Change-Id: Idcf7c9e375f574ec3711ed16a1959925ee3343af
diff --git a/frc971/wpilib/wpilib_interface.cc b/frc971/wpilib/wpilib_interface.cc
new file mode 100644
index 0000000..7827041
--- /dev/null
+++ b/frc971/wpilib/wpilib_interface.cc
@@ -0,0 +1,33 @@
+#include "frc971/wpilib/wpilib_interface.h"
+
+#include "DriverStation.h"
+#include "ControllerPower.h"
+
+#include "aos/common/messages/robot_state.q.h"
+#include "aos/common/logging/queue_logging.h"
+
+namespace frc971 {
+namespace wpilib {
+
+void SendRobotState(int32_t my_pid, DriverStation *ds) {
+ auto new_state = ::aos::robot_state.MakeMessage();
+
+ new_state->reader_pid = my_pid;
+ new_state->outputs_enabled = ds->IsSysActive();
+ new_state->browned_out = ds->IsSysBrownedOut();
+
+ new_state->is_3v3_active = ControllerPower::GetEnabled3V3();
+ new_state->is_5v_active = ControllerPower::GetEnabled5V();
+ new_state->voltage_3v3 = ControllerPower::GetVoltage3V3();
+ new_state->voltage_5v = ControllerPower::GetVoltage5V();
+
+ new_state->voltage_roborio_in = ControllerPower::GetInputVoltage();
+ new_state->voltage_battery = ds->GetBatteryVoltage();
+
+ LOG_STRUCT(DEBUG, "robot_state", *new_state);
+
+ new_state.Send();
+}
+
+} // namespace wpilib
+} // namespace frc971