Deduplicate code for sending out RobotState messages
Change-Id: Idcf7c9e375f574ec3711ed16a1959925ee3343af
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();