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_
diff --git a/y2014/wpilib/BUILD b/y2014/wpilib/BUILD
index 3436376..c9aca05 100644
--- a/y2014/wpilib/BUILD
+++ b/y2014/wpilib/BUILD
@@ -31,5 +31,6 @@
'//frc971/wpilib:encoder_and_potentiometer',
'//frc971/control_loops:queues',
'//frc971/wpilib:logging_queue',
+ '//frc971/wpilib:wpilib_interface',
],
)
diff --git a/y2014/wpilib/wpilib_interface.cc b/y2014/wpilib/wpilib_interface.cc
index 59cced3..4352814 100644
--- a/y2014/wpilib/wpilib_interface.cc
+++ b/y2014/wpilib/wpilib_interface.cc
@@ -15,7 +15,6 @@
#include "Relay.h"
#include "RobotBase.h"
#include "dma.h"
-#include "ControllerPower.h"
#ifndef WPILIB2015
#include "DigitalGlitchFilter.h"
#endif
@@ -32,6 +31,7 @@
#include "aos/common/messages/robot_state.q.h"
#include "frc971/shifter_hall_effect.h"
+
#include "y2014/control_loops/drivetrain/drivetrain.q.h"
#include "y2014/control_loops/claw/claw.q.h"
#include "y2014/control_loops/shooter/shooter.q.h"
@@ -47,6 +47,7 @@
#include "frc971/wpilib/interrupt_edge_counting.h"
#include "frc971/wpilib/encoder_and_potentiometer.h"
#include "frc971/wpilib/logging.q.h"
+#include "frc971/wpilib/wpilib_interface.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
@@ -261,25 +262,7 @@
}
void RunIteration() {
- {
- 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();
- }
+ ::frc971::wpilib::SendRobotState(my_pid_, ds_);
const auto &values = constants::GetValues();
diff --git a/y2014_bot3/wpilib/BUILD b/y2014_bot3/wpilib/BUILD
index 228003d..61075c7 100644
--- a/y2014_bot3/wpilib/BUILD
+++ b/y2014_bot3/wpilib/BUILD
@@ -24,6 +24,7 @@
'//frc971/wpilib:gyro_sender',
'//frc971/control_loops:queues',
'//frc971/wpilib:logging_queue',
+ '//frc971/wpilib:wpilib_interface',
'//y2014_bot3/autonomous:auto_queue',
'//y2014_bot3/control_loops/drivetrain:drivetrain_lib',
'//y2014_bot3/control_loops/rollers:rollers_lib',
diff --git a/y2014_bot3/wpilib/wpilib_interface.cc b/y2014_bot3/wpilib/wpilib_interface.cc
index 4dc421d..096f526 100644
--- a/y2014_bot3/wpilib/wpilib_interface.cc
+++ b/y2014_bot3/wpilib/wpilib_interface.cc
@@ -15,7 +15,6 @@
#include "Relay.h"
#include "RobotBase.h"
#include "dma.h"
-#include "ControllerPower.h"
#include "DigitalInput.h"
#undef ERROR
@@ -32,6 +31,8 @@
#include "y2014_bot3/control_loops/drivetrain/drivetrain.q.h"
#include "y2014_bot3/control_loops/rollers/rollers.q.h"
#include "y2014_bot3/autonomous/auto.q.h"
+#include "y2014_bot3/control_loops/drivetrain/drivetrain.h"
+#include "y2014_bot3/control_loops/rollers/rollers.h"
#include "frc971/wpilib/joystick_sender.h"
#include "frc971/wpilib/loop_output_handler.h"
@@ -39,8 +40,7 @@
#include "frc971/wpilib/buffered_pcm.h"
#include "frc971/wpilib/gyro_sender.h"
#include "frc971/wpilib/logging.q.h"
-#include "y2014_bot3/control_loops/drivetrain/drivetrain.h"
-#include "y2014_bot3/control_loops/rollers/rollers.h"
+#include "frc971/wpilib/wpilib_interface.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
@@ -99,26 +99,7 @@
}
void RunIteration() {
- // General
- {
- 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();
- }
+ ::frc971::wpilib::SendRobotState(my_pid_, ds_);
// Drivetrain
{
diff --git a/y2015/wpilib/BUILD b/y2015/wpilib/BUILD
index 85f9139..c5c4202 100644
--- a/y2015/wpilib/BUILD
+++ b/y2015/wpilib/BUILD
@@ -32,5 +32,6 @@
'//frc971/wpilib:encoder_and_potentiometer',
'//frc971/control_loops:queues',
'//frc971/wpilib:logging_queue',
+ '//frc971/wpilib:wpilib_interface',
],
)
diff --git a/y2015/wpilib/wpilib_interface.cc b/y2015/wpilib/wpilib_interface.cc
index c56d211..c1d9628 100644
--- a/y2015/wpilib/wpilib_interface.cc
+++ b/y2015/wpilib/wpilib_interface.cc
@@ -15,7 +15,6 @@
#include "Relay.h"
#include "RobotBase.h"
#include "dma.h"
-#include "ControllerPower.h"
#ifndef WPILIB2015
#include "DigitalGlitchFilter.h"
#endif
@@ -31,8 +30,9 @@
#include "aos/linux_code/init.h"
#include "aos/common/messages/robot_state.q.h"
-#include "y2015/constants.h"
#include "frc971/control_loops/control_loops.q.h"
+
+#include "y2015/constants.h"
#include "y2015/control_loops/drivetrain/drivetrain.q.h"
#include "y2015/control_loops/fridge/fridge.q.h"
#include "y2015/control_loops/claw/claw.q.h"
@@ -47,6 +47,7 @@
#include "frc971/wpilib/interrupt_edge_counting.h"
#include "frc971/wpilib/encoder_and_potentiometer.h"
#include "frc971/wpilib/logging.q.h"
+#include "frc971/wpilib/wpilib_interface.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
@@ -243,25 +244,7 @@
}
void RunIteration() {
- {
- 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();
- }
+ ::frc971::wpilib::SendRobotState(my_pid_, ds_);
{
auto drivetrain_message = drivetrain_queue.position.MakeMessage();
diff --git a/y2015_bot3/wpilib/BUILD b/y2015_bot3/wpilib/BUILD
index 3e7453b..f89a2b4 100644
--- a/y2015_bot3/wpilib/BUILD
+++ b/y2015_bot3/wpilib/BUILD
@@ -22,6 +22,7 @@
'//frc971/wpilib:loop_output_handler',
'//frc971/wpilib:buffered_pcm',
'//frc971/wpilib:gyro_sender',
+ '//frc971/wpilib:wpilib_interface',
'//frc971/control_loops:queues',
'//frc971/wpilib:logging_queue',
'//y2015_bot3/autonomous:auto_queue',
diff --git a/y2015_bot3/wpilib/wpilib_interface.cc b/y2015_bot3/wpilib/wpilib_interface.cc
index f43fe8e..4dfae3f 100644
--- a/y2015_bot3/wpilib/wpilib_interface.cc
+++ b/y2015_bot3/wpilib/wpilib_interface.cc
@@ -15,7 +15,6 @@
#include "Relay.h"
#include "RobotBase.h"
#include "dma.h"
-#include "ControllerPower.h"
#ifndef WPILIB2015
#include "DigitalGlitchFilter.h"
#endif
@@ -33,10 +32,14 @@
#include "aos/common/messages/robot_state.q.h"
#include "frc971/control_loops/control_loops.q.h"
+
#include "y2015_bot3/control_loops/drivetrain/drivetrain.q.h"
#include "y2015_bot3/control_loops/elevator/elevator.q.h"
#include "y2015_bot3/control_loops/intake/intake.q.h"
#include "y2015_bot3/autonomous/auto.q.h"
+#include "y2015_bot3/control_loops/drivetrain/drivetrain.h"
+#include "y2015_bot3/control_loops/elevator/elevator.h"
+#include "y2015_bot3/control_loops/intake/intake.h"
#include "frc971/wpilib/joystick_sender.h"
#include "frc971/wpilib/loop_output_handler.h"
@@ -44,10 +47,7 @@
#include "frc971/wpilib/buffered_pcm.h"
#include "frc971/wpilib/gyro_sender.h"
#include "frc971/wpilib/logging.q.h"
-#include "y2015_bot3/control_loops/drivetrain/drivetrain.h"
-#include "y2015_bot3/control_loops/elevator/elevator.h"
-#include "y2015_bot3/control_loops/intake/intake.h"
-
+#include "frc971/wpilib/wpilib_interface.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
@@ -139,26 +139,7 @@
}
void RunIteration() {
- // General
- {
- 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();
- }
+ ::frc971::wpilib::SendRobotState(my_pid_, ds_);
// Drivetrain
{