Deduplicate code for sending out RobotState messages
Change-Id: Idcf7c9e375f574ec3711ed16a1959925ee3343af
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index a47fbc4..729c5ca 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -140,3 +140,18 @@
'//aos/common/logging:queue_logging',
],
)
+
+cc_library(
+ name = 'wpilib_interface',
+ srcs = [
+ 'wpilib_interface.cc',
+ ],
+ hdrs = [
+ 'wpilib_interface.h',
+ ],
+ deps = [
+ '//aos/common/messages:robot_state',
+ '//aos/externals:wpilib',
+ '//aos/common/logging:queue_logging',
+ ],
+)
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
diff --git a/frc971/wpilib/wpilib_interface.h b/frc971/wpilib/wpilib_interface.h
new file mode 100644
index 0000000..216bf09
--- /dev/null
+++ b/frc971/wpilib/wpilib_interface.h
@@ -0,0 +1,17 @@
+#ifndef FRC971_WPILIB_WPILIB_INTERFACE_H_
+#define FRC971_WPILIB_WPILIB_INTERFACE_H_
+
+#include <stdint.h>
+
+class DriverStation;
+
+namespace frc971 {
+namespace wpilib {
+
+// Sends out a message on ::aos::robot_state.
+void SendRobotState(int32_t my_pid, DriverStation *ds);
+
+} // namespace wpilib
+} // namespace frc971
+
+#endif // FRC971_WPILIB_WPILIB_INTERFACE_H_