Move PDP values out to a separate queue so the timestamps make sense etc
Change-Id: Iad5303c3446dc29f339f02af9c9c99dd7c75d8e0
diff --git a/aos/common/messages/robot_state.q b/aos/common/messages/robot_state.q
index ab269bd..3ecb653 100644
--- a/aos/common/messages/robot_state.q
+++ b/aos/common/messages/robot_state.q
@@ -33,14 +33,6 @@
// joystick code hasn't died.
queue JoystickState joystick_state;
-// Values retrieved from the PDP.
-struct PDPValues {
- double voltage;
- double temperature;
- double power;
- double[16] currents;
-};
-
message RobotState {
// The PID of the process reading sensors.
// This is here so control loops can tell when it changes.
@@ -67,8 +59,6 @@
// From the DriverStation object, aka what FMS sees and what shows up on the
// actual driver's station.
double voltage_battery;
-
- PDPValues pdp;
};
// Messages are sent out on this queue along with reading sensors. It contains
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index afa9a4e..0e31d83 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -170,7 +170,13 @@
'//aos/common/messages:robot_state',
'//aos/externals:wpilib',
'//aos/common/logging:queue_logging',
- ':pdp_fetcher',
+ ],
+)
+
+queue_library(
+ name = 'pdp_values',
+ srcs = [
+ 'pdp_values.q',
],
)
@@ -183,7 +189,7 @@
'pdp_fetcher.h',
],
deps = [
- '//aos/common/messages:robot_state',
+ ':pdp_values',
'//aos/externals:wpilib',
'//aos/common/logging:queue_logging',
'//aos/linux_code:init',
diff --git a/frc971/wpilib/pdp_fetcher.cc b/frc971/wpilib/pdp_fetcher.cc
index 735036e..4e6be32 100644
--- a/frc971/wpilib/pdp_fetcher.cc
+++ b/frc971/wpilib/pdp_fetcher.cc
@@ -3,21 +3,14 @@
#include "aos/common/logging/queue_logging.h"
#include "aos/linux_code/init.h"
#include "aos/common/util/phased_loop.h"
+#include "frc971/wpilib/pdp_values.q.h"
namespace frc971 {
namespace wpilib {
-PDPFetcher::PDPFetcher() : pdp_(new PowerDistributionPanel()) {
- pdp_values_.Zero();
-}
-
-void PDPFetcher::GetValues(::aos::PDPValues *pdp_values) {
- ::aos::MutexLocker locker(&values_lock_);
- *pdp_values = pdp_values_;
-}
-
void PDPFetcher::operator()() {
::aos::SetCurrentThreadName("PDPFetcher");
+ ::std::unique_ptr<PowerDistributionPanel> pdp(new PowerDistributionPanel());
::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(20),
::aos::time::Time::InMS(4));
@@ -29,29 +22,16 @@
LOG(DEBUG, "PDPFetcher skipped %d iterations\n", iterations - 1);
}
}
- {
- const double voltage = pdp_->GetVoltage();
- ::aos::MutexLocker locker(&values_lock_);
- pdp_values_.voltage = voltage;
- }
- {
- const double temperature = pdp_->GetTemperature();
- ::aos::MutexLocker locker(&values_lock_);
- pdp_values_.temperature = temperature;
- }
- {
- const double power = pdp_->GetTotalPower();
- ::aos::MutexLocker locker(&values_lock_);
- pdp_values_.power = power;
- }
+ auto message = pdp_values.MakeMessage();
+ message->voltage = pdp->GetVoltage();
+ message->temperature = pdp->GetTemperature();
+ message->power = pdp->GetTotalPower();
for (int i = 0; i < 16; ++i) {
- const double current = pdp_->GetCurrent(i);
- ::aos::MutexLocker locker(&values_lock_);
- pdp_values_.currents[i] = current;
+ message->currents[i] = pdp->GetCurrent(i);
}
- {
- ::aos::MutexLocker locker(&values_lock_);
- LOG_STRUCT(DEBUG, "finished fetching", pdp_values_);
+ LOG_STRUCT(DEBUG, "got", *message);
+ if (!message.Send()) {
+ LOG(WARNING, "sending pdp values failed\n");
}
}
}
diff --git a/frc971/wpilib/pdp_fetcher.h b/frc971/wpilib/pdp_fetcher.h
index 8271f2b..5979f96 100644
--- a/frc971/wpilib/pdp_fetcher.h
+++ b/frc971/wpilib/pdp_fetcher.h
@@ -4,9 +4,6 @@
#include <memory>
#include <atomic>
-#include "aos/common/messages/robot_state.q.h"
-#include "aos/common/mutex.h"
-
#include "PowerDistributionPanel.h"
namespace frc971 {
@@ -16,23 +13,12 @@
// separate thread.
class PDPFetcher {
public:
- PDPFetcher();
-
void Quit() { run_ = false; }
- // Retrieves the latest set of values and stores it in *pdp_values.
- // This is safe to call from any thread.
- void GetValues(::aos::PDPValues *pdp_values);
-
// To be called by a ::std::thread.
void operator()();
private:
- const ::std::unique_ptr<PowerDistributionPanel> pdp_;
-
- ::aos::PDPValues pdp_values_;
- ::aos::Mutex values_lock_;
-
::std::atomic<bool> run_{true};
};
diff --git a/frc971/wpilib/pdp_values.q b/frc971/wpilib/pdp_values.q
new file mode 100644
index 0000000..bda1c49
--- /dev/null
+++ b/frc971/wpilib/pdp_values.q
@@ -0,0 +1,11 @@
+package frc971;
+
+// Values retrieved from the PDP.
+message PDPValues {
+ double voltage;
+ double temperature;
+ double power;
+ double[16] currents;
+};
+
+queue PDPValues pdp_values;
diff --git a/frc971/wpilib/wpilib_interface.cc b/frc971/wpilib/wpilib_interface.cc
index 8d67cf0..016f52f 100644
--- a/frc971/wpilib/wpilib_interface.cc
+++ b/frc971/wpilib/wpilib_interface.cc
@@ -3,8 +3,6 @@
#include "aos/common/messages/robot_state.q.h"
#include "aos/common/logging/queue_logging.h"
-#include "frc971/wpilib/pdp_fetcher.h"
-
#include "DriverStation.h"
#include "ControllerPower.h"
#undef ERROR
@@ -12,8 +10,7 @@
namespace frc971 {
namespace wpilib {
-void SendRobotState(int32_t my_pid, DriverStation *ds,
- PDPFetcher *pdp_fetcher) {
+void SendRobotState(int32_t my_pid, DriverStation *ds) {
auto new_state = ::aos::robot_state.MakeMessage();
new_state->reader_pid = my_pid;
@@ -28,10 +25,6 @@
new_state->voltage_roborio_in = ControllerPower::GetInputVoltage();
new_state->voltage_battery = ds->GetBatteryVoltage();
- if (pdp_fetcher) {
- pdp_fetcher->GetValues(&new_state->pdp);
- }
-
LOG_STRUCT(DEBUG, "robot_state", *new_state);
new_state.Send();
diff --git a/frc971/wpilib/wpilib_interface.h b/frc971/wpilib/wpilib_interface.h
index 82936ec..216bf09 100644
--- a/frc971/wpilib/wpilib_interface.h
+++ b/frc971/wpilib/wpilib_interface.h
@@ -8,11 +8,8 @@
namespace frc971 {
namespace wpilib {
-class PDPFetcher;
-
// Sends out a message on ::aos::robot_state.
-void SendRobotState(int32_t my_pid, DriverStation *ds,
- PDPFetcher *pdp_fetcher);
+void SendRobotState(int32_t my_pid, DriverStation *ds);
} // namespace wpilib
} // namespace frc971
diff --git a/y2012/wpilib/wpilib_interface.cc b/y2012/wpilib/wpilib_interface.cc
index 6ed3bb3..a9b7bf6 100644
--- a/y2012/wpilib/wpilib_interface.cc
+++ b/y2012/wpilib/wpilib_interface.cc
@@ -120,7 +120,7 @@
}
void RunIteration() {
- ::frc971::wpilib::SendRobotState(my_pid_, ds_, nullptr);
+ ::frc971::wpilib::SendRobotState(my_pid_, ds_);
{
auto drivetrain_message = drivetrain_queue.position.MakeMessage();
diff --git a/y2014/wpilib/wpilib_interface.cc b/y2014/wpilib/wpilib_interface.cc
index cc634d9..13b2e7b 100644
--- a/y2014/wpilib/wpilib_interface.cc
+++ b/y2014/wpilib/wpilib_interface.cc
@@ -122,8 +122,7 @@
class SensorReader {
public:
- SensorReader(::frc971::wpilib::PDPFetcher *pdp_fetcher)
- : pdp_fetcher_(pdp_fetcher) {
+ SensorReader() {
// Set it to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
encoder_filter_.SetPeriodNanoSeconds(
@@ -280,7 +279,7 @@
}
void RunIteration() {
- ::frc971::wpilib::SendRobotState(my_pid_, ds_, pdp_fetcher_);
+ ::frc971::wpilib::SendRobotState(my_pid_, ds_);
const auto &values = constants::GetValues();
@@ -440,7 +439,6 @@
int32_t my_pid_;
DriverStation *ds_;
- ::frc971::wpilib::PDPFetcher *const pdp_fetcher_;
::std::unique_ptr<::frc971::wpilib::DMASynchronizer> dma_synchronizer_;
@@ -706,7 +704,7 @@
::frc971::wpilib::PDPFetcher pdp_fetcher;
::std::thread pdp_fetcher_thread(::std::ref(pdp_fetcher));
- SensorReader reader(&pdp_fetcher);
+ SensorReader reader;
// Create this first to make sure it ends up in one of the lower-numbered
// FPGA slots so we can use it with DMA.
diff --git a/y2014_bot3/wpilib/wpilib_interface.cc b/y2014_bot3/wpilib/wpilib_interface.cc
index dc3999f..a3e0c27 100644
--- a/y2014_bot3/wpilib/wpilib_interface.cc
+++ b/y2014_bot3/wpilib/wpilib_interface.cc
@@ -74,8 +74,7 @@
// Reads in our inputs. (sensors, voltages, etc.)
class SensorReader {
public:
- SensorReader(::frc971::wpilib::PDPFetcher *pdp_fetcher)
- : pdp_fetcher_(pdp_fetcher) {}
+ SensorReader() {}
void set_drivetrain_left_encoder(::std::unique_ptr<Encoder> encoder) {
drivetrain_left_encoder_ = ::std::move(encoder);
@@ -114,7 +113,7 @@
}
void RunIteration() {
- ::frc971::wpilib::SendRobotState(my_pid_, ds_, pdp_fetcher_);
+ ::frc971::wpilib::SendRobotState(my_pid_, ds_);
// Drivetrain
{
@@ -143,7 +142,6 @@
private:
int32_t my_pid_;
DriverStation *ds_;
- ::frc971::wpilib::PDPFetcher *const pdp_fetcher_;
::std::unique_ptr<Encoder> drivetrain_left_encoder_;
::std::unique_ptr<Encoder> drivetrain_right_encoder_;
@@ -364,7 +362,7 @@
// the robot before turning on.
// Sensors
- SensorReader reader(&pdp_fetcher);
+ SensorReader reader;
reader.set_drivetrain_left_encoder(make_encoder(4));
reader.set_drivetrain_right_encoder(make_encoder(5));
diff --git a/y2015/wpilib/wpilib_interface.cc b/y2015/wpilib/wpilib_interface.cc
index e70c92f..c2250bb 100644
--- a/y2015/wpilib/wpilib_interface.cc
+++ b/y2015/wpilib/wpilib_interface.cc
@@ -126,8 +126,7 @@
class SensorReader {
public:
- SensorReader(::frc971::wpilib::PDPFetcher *pdp_fetcher)
- : pdp_fetcher_(pdp_fetcher) {
+ SensorReader() {
// Set it to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
filter_.SetPeriodNanoSeconds(
@@ -260,7 +259,7 @@
}
void RunIteration() {
- ::frc971::wpilib::SendRobotState(my_pid_, ds_, pdp_fetcher_);
+ ::frc971::wpilib::SendRobotState(my_pid_, ds_);
{
auto drivetrain_message = drivetrain_queue.position.MakeMessage();
@@ -314,7 +313,6 @@
private:
int32_t my_pid_;
DriverStation *ds_;
- ::frc971::wpilib::PDPFetcher *const pdp_fetcher_;
void CopyPotAndIndexPosition(
const DMAEncoderAndPotentiometer &encoder, PotAndIndexPosition *position,
@@ -663,7 +661,7 @@
::frc971::wpilib::PDPFetcher pdp_fetcher;
::std::thread pdp_fetcher_thread(::std::ref(pdp_fetcher));
- SensorReader reader(&pdp_fetcher);
+ SensorReader reader;
reader.set_arm_left_encoder(encoder(1));
reader.set_arm_left_index(make_unique<DigitalInput>(1));
reader.set_arm_left_potentiometer(make_unique<AnalogInput>(1));
diff --git a/y2015_bot3/wpilib/wpilib_interface.cc b/y2015_bot3/wpilib/wpilib_interface.cc
index 6eee1ef..ad87fa5 100644
--- a/y2015_bot3/wpilib/wpilib_interface.cc
+++ b/y2015_bot3/wpilib/wpilib_interface.cc
@@ -94,8 +94,7 @@
// Reads in our inputs. (sensors, voltages, etc.)
class SensorReader {
public:
- SensorReader(::frc971::wpilib::PDPFetcher *pdp_fetcher)
- : pdp_fetcher_(pdp_fetcher) {
+ SensorReader() {
// Set it to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
filter_.SetPeriodNanoSeconds(
@@ -154,7 +153,7 @@
}
void RunIteration() {
- ::frc971::wpilib::SendRobotState(my_pid_, ds_, pdp_fetcher_);
+ ::frc971::wpilib::SendRobotState(my_pid_, ds_);
// Drivetrain
{
@@ -195,7 +194,6 @@
private:
int32_t my_pid_;
DriverStation *ds_;
- ::frc971::wpilib::PDPFetcher *const pdp_fetcher_;
::std::unique_ptr<Encoder> left_encoder_, right_encoder_, elevator_encoder_;
::std::unique_ptr<DigitalInput> zeroing_hall_effect_;
@@ -478,7 +476,7 @@
::frc971::wpilib::PDPFetcher pdp_fetcher;
::std::thread pdp_fetcher_thread(::std::ref(pdp_fetcher));
- SensorReader reader(&pdp_fetcher);
+ SensorReader reader;
reader.set_elevator_encoder(encoder(6));
reader.set_elevator_zeroing_hall_effect(make_unique<DigitalInput>(6));