commit 699f0cbb1abd562595f3bb4cc8ccbc20c9c97910
author Brian Silverman <brians> Thu Feb 05 19:45:01 2015 -0500
committer Brian Silverman <brians> Sun Feb 08 18:23:58 2015 -0500
tree 8323b43fb356b64f079e78e83a31650a5d6a644a
parent a2e3570010fe989d1cde076107866daae7f05abb

update the information about enabled vs not

With the roboRIO, we get different information about whether motors are
enabled (and some about sensors too), so the system for propagating it
around to everything needs to change.

Change-Id: I7e5f9591eeac1fdfe57b271333c3827431fbef01

frc971/wpilib/wpilib_interface.cc

diff --git a/frc971/wpilib/wpilib_interface.cc b/frc971/wpilib/wpilib_interface.cc
index 554993a..d950c02 100644
--- a/frc971/wpilib/wpilib_interface.cc
+++ b/frc971/wpilib/wpilib_interface.cc
@@ -7,17 +7,15 @@
 #include <mutex>
 #include <functional>
 
-#include "aos/common/controls/output_check.q.h"
-#include "aos/common/controls/sensor_generation.q.h"
 #include "aos/common/logging/logging.h"
 #include "aos/common/logging/queue_logging.h"
-#include "aos/common/messages/robot_state.q.h"
 #include "aos/common/time.h"
 #include "aos/common/util/log_interval.h"
 #include "aos/common/util/phased_loop.h"
 #include "aos/common/util/wrapping_counter.h"
 #include "aos/common/stl_mutex.h"
 #include "aos/linux_code/init.h"
+#include "aos/common/messages/robot_state.q.h"
 
 #include "frc971/control_loops/control_loops.q.h"
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
@@ -41,6 +39,7 @@
 #include "Compressor.h"
 #include "RobotBase.h"
 #include "dma.h"
+#include "ControllerPower.h"
 
 #ifndef M_PI
 #define M_PI 3.14159265358979323846
@@ -213,6 +212,9 @@
   void operator()() {
     ::aos::SetCurrentThreadName("SensorReader");
 
+    my_pid_ = getpid();
+    ds_ = DriverStation::GetInstance();
+
     wrist_encoder_.Start();
     dma_synchronizer_->Start();
 
@@ -226,27 +228,26 @@
   }
 
   void RunIteration() {
-    DriverStation *ds = DriverStation::GetInstance();
+    {
+      auto new_state = ::aos::robot_state.MakeMessage();
 
-    if (ds->IsSysActive()) {
-      auto message = ::aos::controls::output_check_received.MakeMessage();
-      // TODO(brians): Actually read a pulse value from the roboRIO.
-      message->pwm_value = 0;
-      message->pulse_length = -1;
-      LOG_STRUCT(DEBUG, "received", *message);
-      message.Send();
+      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();
+
+      new_state.Send();
     }
 
     drivetrain_queue.position.MakeWithBuilder()
         .right_encoder(drivetrain_translate(right_encoder_->GetRaw()))
         .left_encoder(-drivetrain_translate(left_encoder_->GetRaw()))
-        .battery_voltage(ds->GetBatteryVoltage())
-        .Send();
-
-    // Signal that we are alive.
-    ::aos::controls::sensor_generation.MakeWithBuilder()
-        .reader_pid(getpid())
-        .cape_resets(0)
         .Send();
 
     dma_synchronizer_->RunIteration();
@@ -280,6 +281,9 @@
   static const int kPriority = 30;
   static const int kInterruptPriority = 55;
 
+  int32_t my_pid_;
+  DriverStation *ds_;
+
   void CopyPotAndIndexPosition(
       const DMAEncoderAndPotentiometer &encoder, PotAndIndexPosition *position,
       ::std::function<double(int32_t)> encoder_translate,