Remove the final users of robot_state and joystick_state
This means we can remove them from the .q file.
Change-Id: Iefded3cf4537b2635341f3248c5f50af1534a241
diff --git a/aos/controls/BUILD b/aos/controls/BUILD
index 8c44823..30fe5c6 100644
--- a/aos/controls/BUILD
+++ b/aos/controls/BUILD
@@ -27,6 +27,7 @@
"control_loop_test.h",
],
deps = [
+ "//aos/events:shm-event-loop",
"//aos/logging:queue_logging",
"//aos/robot_state",
"//aos/testing:googletest",
diff --git a/aos/controls/control_loop_test.h b/aos/controls/control_loop_test.h
index 66fb174..95f1c7d 100644
--- a/aos/controls/control_loop_test.h
+++ b/aos/controls/control_loop_test.h
@@ -5,6 +5,7 @@
#include "gtest/gtest.h"
+#include "aos/events/shm-event-loop.h"
#include "aos/logging/queue_logging.h"
#include "aos/robot_state/robot_state.q.h"
#include "aos/testing/test_shm.h"
@@ -22,17 +23,16 @@
class ControlLoopTestTemplated : public TestBaseClass {
public:
ControlLoopTestTemplated() {
- ::aos::joystick_state.Clear();
- ::aos::robot_state.Clear();
+ robot_state_sender_ =
+ test_event_loop_.MakeSender<::aos::RobotState>(".aos.robot_state");
+ joystick_state_sender_ =
+ test_event_loop_.MakeSender<::aos::JoystickState>(".aos.joystick_state");
::aos::time::EnableMockTime(current_time_);
SendMessages(false);
}
virtual ~ControlLoopTestTemplated() {
- ::aos::joystick_state.Clear();
- ::aos::robot_state.Clear();
-
::aos::time::DisableMockTime();
}
@@ -44,7 +44,7 @@
if (current_time_ >= kDSPacketTime + last_ds_time_ ||
last_enabled_ != enabled) {
last_ds_time_ = current_time_;
- auto new_state = ::aos::joystick_state.MakeMessage();
+ auto new_state = joystick_state_sender_.MakeMessage();
new_state->fake = true;
new_state->enabled = enabled;
@@ -56,7 +56,7 @@
}
{
- auto new_state = ::aos::robot_state.MakeMessage();
+ auto new_state = robot_state_sender_.MakeMessage();
new_state->reader_pid = reader_pid_;
new_state->outputs_enabled = enabled;
@@ -113,6 +113,11 @@
::aos::testing::TestSharedMemory my_shm_;
bool last_enabled_ = false;
+
+ ::aos::ShmEventLoop test_event_loop_;
+
+ ::aos::Sender<::aos::RobotState> robot_state_sender_;
+ ::aos::Sender<::aos::JoystickState> joystick_state_sender_;
};
typedef ControlLoopTestTemplated<::testing::Test> ControlLoopTest;
diff --git a/aos/controls/replay_control_loop.h b/aos/controls/replay_control_loop.h
index d1436dd..1626c0a 100644
--- a/aos/controls/replay_control_loop.h
+++ b/aos/controls/replay_control_loop.h
@@ -33,10 +33,10 @@
loop_group_->status.FetchLatest();
loop_group_->output.FetchLatest();
- replayer_.AddDirectQueueSender("wpilib_interface.DSReader",
- "joystick_state", ::aos::joystick_state);
- replayer_.AddDirectQueueSender("wpilib_interface.SensorReader",
- "robot_state", ::aos::robot_state);
+ AddDirectQueueSender<::aos::JoystickState>(
+ "wpilib_interface.DSReader", "joystick_state", ".aos.joystick_state");
+ AddDirectQueueSender<::aos::RobotState>("wpilib_interface.SensorReader",
+ "robot_state", ".aos.robot_state");
replayer_.AddHandler(
process_name, "position",
@@ -49,14 +49,15 @@
::std::function<void(const StatusType &)>(::std::ref(status_)));
// The timing of goal messages doesn't matter, and we don't need to look
// back at them after running the loop.
- replayer_.AddDirectQueueSender(process_name, "goal", loop_group_->goal);
+ AddDirectQueueSender<GoalType>(
+ process_name, "goal", ::std::string(loop_group_->name()) + ".goal");
}
template <class QT>
void AddDirectQueueSender(const ::std::string &process_name,
const ::std::string &log_message,
- const ::aos::Queue<QT> &queue) {
- replayer_.AddDirectQueueSender<QT>(process_name, log_message, queue);
+ const ::std::string &name) {
+ replayer_.AddDirectQueueSender<QT>(process_name, log_message, name);
}
// Replays messages from a file.
@@ -93,6 +94,8 @@
// Returns after going through the entire file.
void DoProcessFile();
+ ::aos::ShmEventLoop event_loop_;
+
T *const loop_group_;
CaptureMessage<PositionType> position_;
diff --git a/aos/events/event-loop.h b/aos/events/event-loop.h
index 90eb721..c827c46 100644
--- a/aos/events/event-loop.h
+++ b/aos/events/event-loop.h
@@ -34,6 +34,8 @@
template <typename T>
class Sender {
public:
+ typedef T Type;
+
Sender() {}
// Represents a single message about to be sent to the queue.
diff --git a/aos/logging/BUILD b/aos/logging/BUILD
index ebc03f6..49bb321 100644
--- a/aos/logging/BUILD
+++ b/aos/logging/BUILD
@@ -13,10 +13,10 @@
visibility = ["//visibility:public"],
deps = [
":sizes",
+ "//aos:complex_thread_local",
"//aos:die",
"//aos:macros",
"//aos/libc:aos_strerror",
- "//aos:complex_thread_local",
],
)
@@ -33,6 +33,7 @@
":binary_log_file",
":logging",
"//aos:queues",
+ "//aos/events:event-loop",
"//aos/ipc_lib:queue",
],
)
@@ -47,12 +48,12 @@
":binary_log_file",
":implementations",
":logging",
- "//aos:die",
- "//aos:queue_types",
- "//aos/time:time",
"//aos:configuration",
+ "//aos:die",
"//aos:init",
+ "//aos:queue_types",
"//aos/ipc_lib:queue",
+ "//aos/time",
],
)
@@ -65,9 +66,9 @@
deps = [
":implementations",
":logging",
- "//aos/time:time",
"//aos:init",
"//aos/ipc_lib:queue",
+ "//aos/time",
],
)
@@ -81,10 +82,10 @@
":binary_log_file",
":implementations",
":logging",
- "//aos:queue_types",
- "//aos/util:string_to_num",
"//aos:configuration",
"//aos:init",
+ "//aos:queue_types",
+ "//aos/util:string_to_num",
],
)
@@ -182,13 +183,13 @@
deps = [
":logging",
":sizes",
- "//aos:once",
"//aos:die",
"//aos:macros",
- "//aos/mutex:mutex",
+ "//aos:once",
"//aos:queue_types",
- "//aos/time:time",
- "//aos/type_traits:type_traits",
"//aos/ipc_lib:queue",
+ "//aos/mutex",
+ "//aos/time",
+ "//aos/type_traits",
],
)
diff --git a/aos/logging/replay.h b/aos/logging/replay.h
index ffb3b7d..679b760 100644
--- a/aos/logging/replay.h
+++ b/aos/logging/replay.h
@@ -6,11 +6,12 @@
#include <functional>
#include <memory>
+#include "aos/events/event-loop.h"
+#include "aos/ipc_lib/queue.h"
#include "aos/logging/binary_log_file.h"
-#include "aos/queue.h"
#include "aos/logging/logging.h"
#include "aos/macros.h"
-#include "aos/ipc_lib/queue.h"
+#include "aos/queue.h"
#include "aos/queue_types.h"
namespace aos {
@@ -68,10 +69,10 @@
template <class T>
void AddDirectQueueSender(const ::std::string &process_name,
const ::std::string &log_message,
- const ::aos::Queue<T> &queue) {
+ const ::std::string &name) {
AddHandler(process_name, log_message,
::std::function<void(const T &)>(
- QueueDumpStructHandler<T>(queue.name())));
+ QueueDumpStructHandler<T>(name.c_str())));
}
private:
diff --git a/aos/queue-tmpl.h b/aos/queue-tmpl.h
index 1362515..2ca79b8 100644
--- a/aos/queue-tmpl.h
+++ b/aos/queue-tmpl.h
@@ -11,16 +11,6 @@
}
template <class T>
-bool ScopedMessagePtr<T>::SendBlocking() {
- assert(msg_ != NULL);
- msg_->SetTimeToNow();
- assert(queue_ != NULL);
- bool return_value = queue_->WriteMessage(msg_, RawQueue::kBlock);
- msg_ = NULL;
- return return_value;
-}
-
-template <class T>
void ScopedMessagePtr<T>::reset(T *msg) {
if (queue_ != NULL && msg_ != NULL) {
queue_->FreeMessage(msg_);
diff --git a/aos/queue.h b/aos/queue.h
index 47540f6..fb11f4f 100644
--- a/aos/queue.h
+++ b/aos/queue.h
@@ -58,12 +58,6 @@
// The message will be freed.
bool Send();
- // Sends the message and removes our reference to it.
- // If the queue is full, block until it is no longer full.
- // Returns true on success, and false otherwise.
- // The message will be freed.
- bool SendBlocking();
-
// Frees the contained message.
~ScopedMessagePtr() {
reset();
diff --git a/aos/robot_state/robot_state.q b/aos/robot_state/robot_state.q
index 7eb10f5..e482c5d 100644
--- a/aos/robot_state/robot_state.q
+++ b/aos/robot_state/robot_state.q
@@ -11,6 +11,8 @@
int32_t pov;
};
+// This message is checked by all control loops to make sure that the
+// joystick code hasn't died. It is published on ".aos.joystick_state"
message JoystickState {
Joystick[6] joysticks;
@@ -34,10 +36,10 @@
bool fake;
};
-// This queue is checked by all control loops to make sure that the
-// joystick code hasn't died.
-queue JoystickState joystick_state;
-
+// This message is sent out on this queue when sensors are read. It contains
+// global robot state and information about whether the process reading sensors
+// has been restarted, along with all counters etc it keeps track of. It is
+// published on ".aos.robot_state"
message RobotState {
// The PID of the process reading sensors.
// This is here so control loops can tell when it changes.
@@ -66,7 +68,3 @@
double voltage_battery;
};
-// Messages are sent out on this queue along with reading sensors. It contains
-// global robot state and information about whether the process reading sensors
-// has been restarted, along with all counters etc it keeps track of.
-queue RobotState robot_state;
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index a1b96be..5f89be1 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -294,6 +294,7 @@
":drivetrain_config",
":drivetrain_queue",
":trajectory",
+ "//aos/events:event-loop",
"//aos/testing:googletest",
"//frc971/control_loops:state_feedback_loop",
"//frc971/queues:gyro",
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 81506f2..412b783 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -5,6 +5,8 @@
#include "aos/controls/control_loop_test.h"
#include "aos/controls/polytope.h"
+#include "aos/events/event-loop.h"
+#include "aos/events/shm-event-loop.h"
#include "aos/time/time.h"
#include "gflags/gflags.h"
#include "gtest/gtest.h"
@@ -42,6 +44,7 @@
DeadReckonEkf localizer_;
// Create a loop and simulation plant.
DrivetrainLoop drivetrain_motor_;
+ ::aos::ShmEventLoop simulation_event_loop_;
DrivetrainSimulation drivetrain_motor_plant_;
DrivetrainTest()
@@ -53,7 +56,7 @@
dt_config_(GetTestDrivetrainConfig()),
localizer_(dt_config_),
drivetrain_motor_(dt_config_, &event_loop_, &localizer_),
- drivetrain_motor_plant_(dt_config_) {
+ drivetrain_motor_plant_(&simulation_event_loop_, dt_config_) {
::frc971::sensors::gyro_reading.Clear();
set_battery_voltage(12.0);
}
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index 49b0387..7617dc2 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -82,8 +82,11 @@
}
DrivetrainSimulation::DrivetrainSimulation(
- const DrivetrainConfig<double> &dt_config)
- : dt_config_(dt_config),
+ ::aos::EventLoop *event_loop, const DrivetrainConfig<double> &dt_config)
+ : event_loop_(event_loop),
+ robot_state_fetcher_(
+ event_loop_->MakeFetcher<::aos::RobotState>(".aos.robot_state")),
+ dt_config_(dt_config),
drivetrain_plant_(MakePlantFromConfig(dt_config_)),
my_drivetrain_queue_(".frc971.control_loops.drivetrain_queue",
".frc971.control_loops.drivetrain_queue.goal",
@@ -148,8 +151,10 @@
last_U_ << my_drivetrain_queue_.output->left_voltage,
my_drivetrain_queue_.output->right_voltage;
{
- ::aos::robot_state.FetchLatest();
- const double scalar = ::aos::robot_state->voltage_battery / 12.0;
+ robot_state_fetcher_.Fetch();
+ const double scalar = robot_state_fetcher_.get()
+ ? robot_state_fetcher_->voltage_battery / 12.0
+ : 1.0;
last_U_ *= scalar;
}
left_gear_high_ = my_drivetrain_queue_.output->left_high;
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.h b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
index a8dfa07..7914b50 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.h
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
@@ -1,6 +1,7 @@
#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_TEST_LIB_H_
#define FRC971_CONTROL_LOOPS_DRIVETRAIN_DRIVETRAIN_TEST_LIB_H_
+#include "aos/events/event-loop.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/state_feedback_loop.h"
@@ -41,7 +42,8 @@
public:
// Constructs a motor simulation.
// TODO(aschuh) Do we want to test the clutch one too?
- DrivetrainSimulation(const DrivetrainConfig<double> &dt_config);
+ DrivetrainSimulation(::aos::EventLoop *event_loop,
+ const DrivetrainConfig<double> &dt_config);
// Resets the plant.
void Reinitialize();
@@ -72,6 +74,9 @@
}
private:
+ ::aos::EventLoop *event_loop_;
+ ::aos::Fetcher<::aos::RobotState> robot_state_fetcher_;
+
DrivetrainConfig<double> dt_config_;
DrivetrainPlant drivetrain_plant_;
diff --git a/frc971/control_loops/drivetrain/replay_drivetrain.cc b/frc971/control_loops/drivetrain/replay_drivetrain.cc
index 697bdec..db058b8 100644
--- a/frc971/control_loops/drivetrain/replay_drivetrain.cc
+++ b/frc971/control_loops/drivetrain/replay_drivetrain.cc
@@ -20,8 +20,8 @@
::frc971::control_loops::DrivetrainQueue>
replayer(&::frc971::control_loops::drivetrain_queue, "drivetrain");
- replayer.AddDirectQueueSender("wpilib_interface.Gyro", "sending",
- ::frc971::sensors::gyro_reading);
+ replayer.AddDirectQueueSender<::frc971::sensors::GyroReading>(
+ "wpilib_interface.Gyro", "sending", ".frc971.sensors.gyro_reading");
for (int i = 1; i < argc; ++i) {
replayer.ProcessFile(argv[i]);
}
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index ad7551f..e54175b 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -154,17 +154,14 @@
// Computes the new X and Y given the control input.
void Update(const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
- ::std::chrono::nanoseconds dt) {
+ ::std::chrono::nanoseconds dt, Scalar voltage_battery) {
// Powers outside of the range are more likely controller bugs than things
// that the plant should deal with.
CheckU(U);
- ::aos::robot_state.FetchLatest();
Eigen::Matrix<Scalar, number_of_inputs, 1> current_U =
- DelayedU_ *
- (::aos::robot_state.get()
- ? ::aos::robot_state->voltage_battery / static_cast<Scalar>(12.0)
- : static_cast<Scalar>(1.0));
+ DelayedU_ * voltage_battery / static_cast<Scalar>(12.0);
+
X_ = Update(X(), current_U, dt);
Y_ = C() * X() + D() * current_U;
DelayedU_ = U;
@@ -176,6 +173,9 @@
const Eigen::Matrix<Scalar, number_of_states, 1> X,
const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
::std::chrono::nanoseconds dt) {
+ // TODO(austin): Hmmm, looks like we aren't compensating for battery voltage
+ // or the unit delay... Might want to do that if we care about performance
+ // again.
UpdateAB(dt);
return A() * X + B() * U;
}
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
index f685e1b..1553a59 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
@@ -171,7 +171,10 @@
using ProfiledJointStatus = typename SZSDPS::ProfiledJointStatus;
IntakeSystemTest()
- : subsystem_(TestIntakeSystemValues<
+ : robot_state_fetcher_(
+ simulation_event_loop_.MakeFetcher<::aos::RobotState>(
+ ".aos.robot_state")),
+ subsystem_(TestIntakeSystemValues<
typename SZSDPS::ZeroingEstimator>::make_params()),
subsystem_plant_() {}
@@ -190,14 +193,14 @@
// Checks if the robot was reset and resets the subsystem.
// Required since there is no ControlLoop to reset it (ie. a superstructure)
- ::aos::robot_state.FetchLatest();
- if (::aos::robot_state.get() &&
- sensor_reader_pid_ != ::aos::robot_state->reader_pid) {
- LOG(ERROR, "WPILib reset, restarting\n");
- subsystem_.Reset();
+ robot_state_fetcher_.Fetch();
+ if (robot_state_fetcher_.get()) {
+ if (sensor_reader_pid_ != robot_state_fetcher_->reader_pid) {
+ LOG(ERROR, "WPILib reset, restarting\n");
+ subsystem_.Reset();
+ }
+ sensor_reader_pid_ = robot_state_fetcher_->reader_pid;
}
-
- sensor_reader_pid_ = ::aos::robot_state->reader_pid;
subsystem_goal_.unsafe_goal = subsystem_data_.goal.unsafe_goal;
subsystem_goal_.profile_params = subsystem_data_.goal.profile_params;
@@ -244,6 +247,8 @@
// that it points to. Otherwise, we will have a pointer to shared memory
// that is no longer valid.
// TestIntakeSystemData subsystem_data_;
+ ::aos::ShmEventLoop simulation_event_loop_;
+ ::aos::Fetcher<::aos::RobotState> robot_state_fetcher_;
// Create a control loop and simulation.
SZSDPS subsystem_;
diff --git a/frc971/wpilib/ADIS16448.cc b/frc971/wpilib/ADIS16448.cc
index 6566c85..e902691 100644
--- a/frc971/wpilib/ADIS16448.cc
+++ b/frc971/wpilib/ADIS16448.cc
@@ -118,8 +118,13 @@
} // namespace
-ADIS16448::ADIS16448(frc::SPI::Port port, frc::DigitalInput *dio1)
- : spi_(new frc::SPI(port)), dio1_(dio1) {
+ADIS16448::ADIS16448(::aos::EventLoop *event_loop, frc::SPI::Port port,
+ frc::DigitalInput *dio1)
+ : event_loop_(event_loop),
+ joystick_state_fetcher_(event_loop_->MakeFetcher<::aos::JoystickState>(
+ ".aos.joystick_state")),
+ spi_(new frc::SPI(port)),
+ dio1_(dio1) {
// 1MHz is the maximum supported for burst reads, but we
// want to go slower to hopefully make it more reliable.
// Note that the roboRIO's minimum supported clock rate appears to be
@@ -262,8 +267,8 @@
if (average_gyro_x.full() && average_gyro_y.full() &&
average_gyro_z.full()) {
- ::aos::joystick_state.FetchLatest();
- if (::aos::joystick_state.get() && ::aos::joystick_state->enabled) {
+ joystick_state_fetcher_.Fetch();
+ if (joystick_state_fetcher_.get() && joystick_state_fetcher_->enabled) {
gyro_x_zeroed_offset_ = -average_gyro_x.GetAverage();
gyro_y_zeroed_offset_ = -average_gyro_y.GetAverage();
gyro_z_zeroed_offset_ = -average_gyro_z.GetAverage();
diff --git a/frc971/wpilib/ADIS16448.h b/frc971/wpilib/ADIS16448.h
index 49c4fb2..a1d6dcd 100644
--- a/frc971/wpilib/ADIS16448.h
+++ b/frc971/wpilib/ADIS16448.h
@@ -11,7 +11,9 @@
#include "frc971/wpilib/ahal/SPI.h"
#undef ERROR
+#include "aos/events/event-loop.h"
#include "aos/logging/logging.h"
+#include "aos/robot_state/robot_state.q.h"
#include "frc971/wpilib/spi_rx_clearer.h"
namespace frc971 {
@@ -29,7 +31,8 @@
public:
// port is where to find the sensor over SPI.
// dio1 must be connected to DIO1 on the sensor.
- ADIS16448(frc::SPI::Port port, frc::DigitalInput *dio1);
+ ADIS16448(::aos::EventLoop *event_loop, frc::SPI::Port port,
+ frc::DigitalInput *dio1);
// Sets the dummy SPI port to send values on to make the roboRIO deassert the
// chip select line. This is mainly useful when there are no other devices
@@ -89,6 +92,9 @@
// Returns true if it succeeds.
bool Initialize();
+ ::aos::EventLoop *event_loop_;
+ ::aos::Fetcher<::aos::JoystickState> joystick_state_fetcher_;
+
// TODO(Brian): This object has no business owning these ones.
const ::std::unique_ptr<frc::SPI> spi_;
::std::unique_ptr<frc::SPI> dummy_spi_;
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index c2707ec..889c32e 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -107,6 +107,7 @@
deps = [
":gyro_interface",
"//aos:init",
+ "//aos/events:event-loop",
"//aos/logging",
"//aos/logging:queue_logging",
"//aos/robot_state",
@@ -144,6 +145,7 @@
],
deps = [
"//aos:init",
+ "//aos/events:event-loop",
"//aos/robot_state",
"//aos/scoped:scoped_fd",
"//aos/time",
@@ -162,6 +164,7 @@
restricted_to = ["//tools:roborio"],
deps = [
"//aos:init",
+ "//aos/events:shm-event-loop",
"//aos/logging:queue_logging",
"//aos/network:team_number",
"//aos/robot_state",
@@ -241,6 +244,7 @@
":imu_queue",
":spi_rx_clearer",
"//aos:init",
+ "//aos/events:event-loop",
"//aos/logging",
"//aos/logging:queue_logging",
"//aos/robot_state",
@@ -292,6 +296,7 @@
":encoder_and_potentiometer",
":wpilib_interface",
"//aos:init",
+ "//aos/events:event-loop",
"//aos/stl_mutex",
"//aos/time",
"//aos/util:phased_loop",
diff --git a/frc971/wpilib/drivetrain_writer.h b/frc971/wpilib/drivetrain_writer.h
index 61fa0ff..ea3df46 100644
--- a/frc971/wpilib/drivetrain_writer.h
+++ b/frc971/wpilib/drivetrain_writer.h
@@ -11,6 +11,9 @@
class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler {
public:
+ DrivetrainWriter(::aos::EventLoop *event_loop)
+ : ::frc971::wpilib::LoopOutputHandler(event_loop) {}
+
void set_left_controller0(::std::unique_ptr<::frc::PWM> t, bool reversed) {
left_controller0_ = ::std::move(t);
reversed_left0_ = reversed;
diff --git a/frc971/wpilib/gyro_sender.cc b/frc971/wpilib/gyro_sender.cc
index 3cbff00..121665f 100644
--- a/frc971/wpilib/gyro_sender.cc
+++ b/frc971/wpilib/gyro_sender.cc
@@ -7,12 +7,13 @@
#include <chrono>
+#include "aos/events/event-loop.h"
+#include "aos/init.h"
#include "aos/logging/logging.h"
#include "aos/logging/queue_logging.h"
-#include "aos/util/phased_loop.h"
#include "aos/robot_state/robot_state.q.h"
#include "aos/time/time.h"
-#include "aos/init.h"
+#include "aos/util/phased_loop.h"
#include "frc971/queues/gyro.q.h"
#include "frc971/zeroing/averager.h"
@@ -23,9 +24,12 @@
namespace chrono = ::std::chrono;
using ::aos::monotonic_clock;
-GyroSender::GyroSender() {
- PCHECK(system(
- "ps -ef | grep '\\[spi0\\]' | awk '{print $1}' | xargs chrt -f -p "
+GyroSender::GyroSender(::aos::EventLoop *event_loop)
+ : event_loop_(event_loop),
+ joystick_state_fetcher_(event_loop_->MakeFetcher<::aos::JoystickState>(
+ ".aos.joystick_state")) {
+ PCHECK(
+ system("ps -ef | grep '\\[spi0\\]' | awk '{print $1}' | xargs chrt -f -p "
"33") == 0);
}
@@ -135,8 +139,8 @@
}
zeroing_data.AddData(new_angle);
- ::aos::joystick_state.FetchLatest();
- if (::aos::joystick_state.get() && ::aos::joystick_state->enabled &&
+ joystick_state_fetcher_.Fetch();
+ if (joystick_state_fetcher_.get() && joystick_state_fetcher_->enabled &&
zeroing_data.full()) {
zero_offset = -zeroing_data.GetAverage();
LOG(INFO, "total zero offset %f\n", zero_offset);
diff --git a/frc971/wpilib/gyro_sender.h b/frc971/wpilib/gyro_sender.h
index a36fc2e..d41109e 100644
--- a/frc971/wpilib/gyro_sender.h
+++ b/frc971/wpilib/gyro_sender.h
@@ -5,6 +5,8 @@
#include <atomic>
+#include "aos/events/event-loop.h"
+#include "aos/robot_state/robot_state.q.h"
#include "frc971/wpilib/gyro_interface.h"
namespace frc971 {
@@ -16,7 +18,7 @@
// as a separate thread.
class GyroSender {
public:
- GyroSender();
+ GyroSender(::aos::EventLoop *event_loop);
// For ::std::thread to call.
//
@@ -26,6 +28,8 @@
void Quit() { run_ = false; }
private:
+ ::aos::EventLoop *event_loop_;
+ ::aos::Fetcher<::aos::JoystickState> joystick_state_fetcher_;
// Readings per second.
static const int kReadingRate = 200;
diff --git a/frc971/wpilib/joystick_sender.cc b/frc971/wpilib/joystick_sender.cc
index 8ca098c..a71ab9c 100644
--- a/frc971/wpilib/joystick_sender.cc
+++ b/frc971/wpilib/joystick_sender.cc
@@ -22,7 +22,7 @@
// variable / mutex needs to get exposed all the way out or something).
while (run_) {
ds->RunIteration([&]() {
- auto new_state = ::aos::joystick_state.MakeMessage();
+ auto new_state = joystick_state_sender_.MakeMessage();
HAL_MatchInfo match_info;
auto status = HAL_GetMatchInfo(&match_info);
diff --git a/frc971/wpilib/joystick_sender.h b/frc971/wpilib/joystick_sender.h
index 205f9c4..a758173 100644
--- a/frc971/wpilib/joystick_sender.h
+++ b/frc971/wpilib/joystick_sender.h
@@ -3,16 +3,25 @@
#include <atomic>
+#include "aos/events/event-loop.h"
+#include "aos/robot_state/robot_state.q.h"
+
namespace frc971 {
namespace wpilib {
class JoystickSender {
public:
+ JoystickSender(::aos::EventLoop *event_loop)
+ : event_loop_(event_loop),
+ joystick_state_sender_(event_loop_->MakeSender<::aos::JoystickState>(
+ ".aos.joystick_state")) {}
void operator()();
void Quit() { run_ = false; }
private:
+ ::aos::EventLoop *event_loop_;
+ ::aos::Sender<::aos::JoystickState> joystick_state_sender_;
::std::atomic<bool> run_{true};
};
diff --git a/frc971/wpilib/loop_output_handler.cc b/frc971/wpilib/loop_output_handler.cc
index 7861760..6be855e 100644
--- a/frc971/wpilib/loop_output_handler.cc
+++ b/frc971/wpilib/loop_output_handler.cc
@@ -6,6 +6,7 @@
#include <functional>
#include <thread>
+#include "aos/events/event-loop.h"
#include "aos/init.h"
#include "aos/robot_state/robot_state.q.h"
@@ -14,8 +15,12 @@
namespace chrono = ::std::chrono;
-LoopOutputHandler::LoopOutputHandler(::std::chrono::nanoseconds timeout)
- : watchdog_(this, timeout) {}
+LoopOutputHandler::LoopOutputHandler(::aos::EventLoop *event_loop,
+ ::std::chrono::nanoseconds timeout)
+ : event_loop_(event_loop),
+ joystick_state_fetcher_(event_loop_->MakeFetcher<::aos::JoystickState>(
+ ".aos.joystick_state")),
+ watchdog_(this, timeout) {}
void LoopOutputHandler::operator()() {
::std::thread watchdog_thread(::std::ref(watchdog_));
@@ -26,12 +31,12 @@
no_joystick_state_.Print();
fake_joystick_state_.Print();
Read();
- ::aos::joystick_state.FetchLatest();
- if (!::aos::joystick_state.get()) {
+ joystick_state_fetcher_.Fetch();
+ if (!joystick_state_fetcher_.get()) {
LOG_INTERVAL(no_joystick_state_);
continue;
}
- if (::aos::joystick_state->fake) {
+ if (joystick_state_fetcher_->fake) {
LOG_INTERVAL(fake_joystick_state_);
continue;
}
diff --git a/frc971/wpilib/loop_output_handler.h b/frc971/wpilib/loop_output_handler.h
index 27c96e7..ff8415f 100644
--- a/frc971/wpilib/loop_output_handler.h
+++ b/frc971/wpilib/loop_output_handler.h
@@ -4,6 +4,8 @@
#include <atomic>
#include <chrono>
+#include "aos/events/event-loop.h"
+#include "aos/robot_state/robot_state.q.h"
#include "aos/scoped/scoped_fd.h"
#include "aos/time/time.h"
#include "aos/util/log_interval.h"
@@ -22,6 +24,7 @@
class LoopOutputHandler {
public:
LoopOutputHandler(
+ ::aos::EventLoop *event_loop,
::std::chrono::nanoseconds timeout = ::std::chrono::milliseconds(100));
void Quit() { run_ = false; }
@@ -43,6 +46,9 @@
// after Quit is called.
virtual void Stop() = 0;
+ // Returns a pointer to the event loop.
+ ::aos::EventLoop *event_loop() { return event_loop_; }
+
private:
// The thread that actually contains a timerfd to implement timeouts. The
// idea is to have a timerfd that is repeatedly set to the timeout expiration
@@ -72,6 +78,8 @@
::std::atomic<bool> run_{true};
};
+ ::aos::EventLoop *event_loop_;
+ ::aos::Fetcher<::aos::JoystickState> joystick_state_fetcher_;
Watchdog watchdog_;
::std::atomic<bool> run_{true};
diff --git a/frc971/wpilib/sensor_reader.cc b/frc971/wpilib/sensor_reader.cc
index d25db27..ec1e822 100644
--- a/frc971/wpilib/sensor_reader.cc
+++ b/frc971/wpilib/sensor_reader.cc
@@ -4,6 +4,7 @@
#include <unistd.h>
#include "aos/init.h"
+#include "aos/logging/queue_logging.h"
#include "aos/util/compiler_memory_barrier.h"
#include "aos/util/phased_loop.h"
#include "frc971/wpilib/ahal/DigitalInput.h"
@@ -15,7 +16,10 @@
namespace frc971 {
namespace wpilib {
-SensorReader::SensorReader() {
+SensorReader::SensorReader(::aos::EventLoop *event_loop)
+ : event_loop_(event_loop),
+ robot_state_sender_(
+ event_loop_->MakeSender<::aos::RobotState>(".aos.robot_state")) {
// Set some defaults. We don't tend to exceed these, so old robots should
// just work with them.
UpdateFastEncoderFilterHz(500000);
@@ -110,7 +114,12 @@
}
const monotonic_clock::time_point monotonic_now = monotonic_clock::now();
- ::frc971::wpilib::SendRobotState(my_pid);
+ {
+ auto new_state = robot_state_sender_.MakeMessage();
+ ::frc971::wpilib::PopulateRobotState(new_state.get(), my_pid);
+ LOG_STRUCT(DEBUG, "robot_state", *new_state);
+ new_state.Send();
+ }
RunIteration();
if (dma_synchronizer_) {
dma_synchronizer_->RunIteration();
diff --git a/frc971/wpilib/sensor_reader.h b/frc971/wpilib/sensor_reader.h
index 12bcf37..d14f704 100644
--- a/frc971/wpilib/sensor_reader.h
+++ b/frc971/wpilib/sensor_reader.h
@@ -4,6 +4,8 @@
#include <atomic>
#include <chrono>
+#include "aos/events/event-loop.h"
+#include "aos/robot_state/robot_state.q.h"
#include "aos/stl_mutex/stl_mutex.h"
#include "aos/time/time.h"
#include "frc971/control_loops/control_loops.q.h"
@@ -22,7 +24,7 @@
class SensorReader {
public:
- SensorReader();
+ SensorReader(::aos::EventLoop *event_loop);
virtual ~SensorReader() {}
// Updates the fast and medium encoder filter frequencies.
@@ -171,6 +173,9 @@
(2.0 * M_PI);
}
+ ::aos::EventLoop *event_loop_;
+ ::aos::Sender<::aos::RobotState> robot_state_sender_;
+
frc::DigitalGlitchFilter fast_encoder_filter_, medium_encoder_filter_;
::std::unique_ptr<frc::Encoder> drivetrain_left_encoder_,
diff --git a/frc971/wpilib/wpilib_interface.cc b/frc971/wpilib/wpilib_interface.cc
index 4dddd91..851d809 100644
--- a/frc971/wpilib/wpilib_interface.cc
+++ b/frc971/wpilib/wpilib_interface.cc
@@ -1,33 +1,30 @@
#include "frc971/wpilib/wpilib_interface.h"
#include "aos/robot_state/robot_state.q.h"
-#include "aos/logging/queue_logging.h"
#include "hal/HAL.h"
namespace frc971 {
namespace wpilib {
-void SendRobotState(int32_t my_pid) {
- auto new_state = ::aos::robot_state.MakeMessage();
-
+void PopulateRobotState(::aos::RobotState *robot_state, int32_t my_pid) {
int32_t status = 0;
- new_state->reader_pid = my_pid;
- new_state->outputs_enabled = HAL_GetSystemActive(&status);
- new_state->browned_out = HAL_GetBrownedOut(&status);
+ robot_state->reader_pid = my_pid;
+ robot_state->outputs_enabled = HAL_GetSystemActive(&status);
+ robot_state->browned_out = HAL_GetBrownedOut(&status);
- new_state->is_3v3_active = HAL_GetUserActive3V3(&status);
- new_state->is_5v_active = HAL_GetUserActive5V(&status);
- new_state->voltage_3v3 = HAL_GetUserVoltage3V3(&status);
- new_state->voltage_5v = HAL_GetUserVoltage5V(&status);
+ robot_state->is_3v3_active = HAL_GetUserActive3V3(&status);
+ robot_state->is_5v_active = HAL_GetUserActive5V(&status);
+ robot_state->voltage_3v3 = HAL_GetUserVoltage3V3(&status);
+ robot_state->voltage_5v = HAL_GetUserVoltage5V(&status);
- new_state->voltage_roborio_in = HAL_GetVinVoltage(&status);
- new_state->voltage_battery = HAL_GetVinVoltage(&status);
+ robot_state->voltage_roborio_in = HAL_GetVinVoltage(&status);
+ robot_state->voltage_battery = HAL_GetVinVoltage(&status);
- LOG_STRUCT(DEBUG, "robot_state", *new_state);
-
- new_state.Send();
+ if (status != 0) {
+ LOG(FATAL, "Failed to get robot state: %d\n", status);
+ }
}
} // namespace wpilib
diff --git a/frc971/wpilib/wpilib_interface.h b/frc971/wpilib/wpilib_interface.h
index 100c67e..5db852a 100644
--- a/frc971/wpilib/wpilib_interface.h
+++ b/frc971/wpilib/wpilib_interface.h
@@ -3,11 +3,13 @@
#include <stdint.h>
+#include "aos/robot_state/robot_state.q.h"
+
namespace frc971 {
namespace wpilib {
// Sends out a message on ::aos::robot_state.
-void SendRobotState(int32_t my_pid);
+void PopulateRobotState(::aos::RobotState *robot_state, int32_t my_pid);
} // namespace wpilib
} // namespace frc971
diff --git a/y2012/BUILD b/y2012/BUILD
index 2841ee4..401beac 100644
--- a/y2012/BUILD
+++ b/y2012/BUILD
@@ -14,7 +14,6 @@
"//aos/util:log_interval",
"//frc971/autonomous:auto_queue",
"//frc971/control_loops/drivetrain:drivetrain_queue",
- "//frc971/queues:gyro",
"//y2012/control_loops/accessories:accessories_queue",
],
)
@@ -38,6 +37,7 @@
"//aos:init",
"//aos:make_unique",
"//aos/controls:control_loop",
+ "//aos/events:shm-event-loop",
"//aos/logging",
"//aos/logging:queue_logging",
"//aos/robot_state",
@@ -53,7 +53,6 @@
"//frc971/wpilib:dma_edge_counting",
"//frc971/wpilib:drivetrain_writer",
"//frc971/wpilib:encoder_and_potentiometer",
- "//frc971/wpilib:gyro_sender",
"//frc971/wpilib:interrupt_edge_counting",
"//frc971/wpilib:joystick_sender",
"//frc971/wpilib:logging_queue",
diff --git a/y2012/wpilib_interface.cc b/y2012/wpilib_interface.cc
index 0fa0f45..98d35c9 100644
--- a/y2012/wpilib_interface.cc
+++ b/y2012/wpilib_interface.cc
@@ -19,6 +19,7 @@
#include "frc971/wpilib/wpilib_robot_base.h"
#undef ERROR
+#include "aos/events/shm-event-loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
#include "aos/logging/queue_logging.h"
@@ -36,7 +37,6 @@
#include "frc971/wpilib/dma_edge_counting.h"
#include "frc971/wpilib/drivetrain_writer.h"
#include "frc971/wpilib/encoder_and_potentiometer.h"
-#include "frc971/wpilib/gyro_sender.h"
#include "frc971/wpilib/interrupt_edge_counting.h"
#include "frc971/wpilib/joystick_sender.h"
#include "frc971/wpilib/logging.q.h"
@@ -71,7 +71,8 @@
class SensorReader : public ::frc971::wpilib::SensorReader {
public:
- SensorReader() {}
+ SensorReader(::aos::EventLoop *event_loop)
+ : ::frc971::wpilib::SensorReader(event_loop) {}
void RunIteration() {
{
@@ -184,6 +185,9 @@
class AccessoriesWriter : public ::frc971::wpilib::LoopOutputHandler {
public:
+ AccessoriesWriter(::aos::EventLoop *event_loop)
+ : ::frc971::wpilib::LoopOutputHandler(event_loop) {}
+
void set_talon1(::std::unique_ptr<Talon> t) {
talon1_ = ::std::move(t);
}
@@ -224,24 +228,26 @@
::aos::InitNRT();
::aos::SetCurrentThreadName("StartCompetition");
- ::frc971::wpilib::JoystickSender joystick_sender;
+ ::aos::ShmEventLoop event_loop;
+
+ ::frc971::wpilib::JoystickSender joystick_sender(&event_loop);
::std::thread joystick_thread(::std::ref(joystick_sender));
- SensorReader reader;
+ SensorReader reader(&event_loop);
reader.set_drivetrain_left_encoder(make_encoder(0));
reader.set_drivetrain_right_encoder(make_encoder(1));
::std::thread reader_thread(::std::ref(reader));
- ::frc971::wpilib::DrivetrainWriter drivetrain_writer;
+ ::frc971::wpilib::DrivetrainWriter drivetrain_writer(&event_loop);
drivetrain_writer.set_left_controller0(
::std::unique_ptr<Talon>(new Talon(3)), true);
drivetrain_writer.set_right_controller0(
::std::unique_ptr<Talon>(new Talon(4)), false);
::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
- ::y2012::wpilib::AccessoriesWriter accessories_writer;
+ ::y2012::wpilib::AccessoriesWriter accessories_writer(&event_loop);
accessories_writer.set_talon1(::std::unique_ptr<Talon>(new Talon(5)));
accessories_writer.set_talon2(::std::unique_ptr<Talon>(new Talon(6)));
::std::thread accessories_writer_thread(::std::ref(accessories_writer));
diff --git a/y2014/wpilib_interface.cc b/y2014/wpilib_interface.cc
index 0ad900e..7807ebd 100644
--- a/y2014/wpilib_interface.cc
+++ b/y2014/wpilib_interface.cc
@@ -18,6 +18,7 @@
#include "frc971/wpilib/wpilib_robot_base.h"
#undef ERROR
+#include "aos/events/shm-event-loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
#include "aos/logging/queue_logging.h"
@@ -117,7 +118,8 @@
class SensorReader : public ::frc971::wpilib::SensorReader {
public:
- SensorReader() {
+ SensorReader(::aos::EventLoop *event_loop)
+ : ::frc971::wpilib::SensorReader(event_loop) {
// Set it to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateMediumEncoderFilterHz(kMaximumEncoderPulsesPerSecond);
@@ -516,6 +518,9 @@
class ShooterWriter : public ::frc971::wpilib::LoopOutputHandler {
public:
+ ShooterWriter(::aos::EventLoop *event_loop)
+ : ::frc971::wpilib::LoopOutputHandler(event_loop) {}
+
void set_shooter_talon(::std::unique_ptr<Talon> t) {
shooter_talon_ = ::std::move(t);
}
@@ -541,6 +546,9 @@
class ClawWriter : public ::frc971::wpilib::LoopOutputHandler {
public:
+ ClawWriter(::aos::EventLoop *event_loop)
+ : ::frc971::wpilib::LoopOutputHandler(event_loop) {}
+
void set_top_claw_talon(::std::unique_ptr<Talon> t) {
top_claw_talon_ = ::std::move(t);
}
@@ -610,12 +618,14 @@
::aos::InitNRT();
::aos::SetCurrentThreadName("StartCompetition");
- ::frc971::wpilib::JoystickSender joystick_sender;
+ ::aos::ShmEventLoop event_loop;
+
+ ::frc971::wpilib::JoystickSender joystick_sender(&event_loop);
::std::thread joystick_thread(::std::ref(joystick_sender));
::frc971::wpilib::PDPFetcher pdp_fetcher;
::std::thread pdp_fetcher_thread(::std::ref(pdp_fetcher));
- SensorReader reader;
+ SensorReader reader(&event_loop);
// Create this first to make sure it ends up in one of the lower-numbered
// FPGA slots so we can use it with DMA.
@@ -648,17 +658,17 @@
::std::thread reader_thread(::std::ref(reader));
- ::frc971::wpilib::GyroSender gyro_sender;
+ ::frc971::wpilib::GyroSender gyro_sender(&event_loop);
::std::thread gyro_thread(::std::ref(gyro_sender));
- ::frc971::wpilib::DrivetrainWriter drivetrain_writer;
+ ::frc971::wpilib::DrivetrainWriter drivetrain_writer(&event_loop);
drivetrain_writer.set_left_controller0(
::std::unique_ptr<Talon>(new Talon(5)), true);
drivetrain_writer.set_right_controller0(
::std::unique_ptr<Talon>(new Talon(2)), false);
::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
- ::y2014::wpilib::ClawWriter claw_writer;
+ ::y2014::wpilib::ClawWriter claw_writer(&event_loop);
claw_writer.set_top_claw_talon(::std::unique_ptr<Talon>(new Talon(1)));
claw_writer.set_bottom_claw_talon(::std::unique_ptr<Talon>(new Talon(0)));
claw_writer.set_left_tusk_talon(::std::unique_ptr<Talon>(new Talon(4)));
@@ -667,7 +677,7 @@
claw_writer.set_intake2_talon(::std::unique_ptr<Talon>(new Talon(8)));
::std::thread claw_writer_thread(::std::ref(claw_writer));
- ::y2014::wpilib::ShooterWriter shooter_writer;
+ ::y2014::wpilib::ShooterWriter shooter_writer(&event_loop);
shooter_writer.set_shooter_talon(::std::unique_ptr<Talon>(new Talon(6)));
::std::thread shooter_writer_thread(::std::ref(shooter_writer));
diff --git a/y2014_bot3/wpilib_interface.cc b/y2014_bot3/wpilib_interface.cc
index 56d72f1..397fafe 100644
--- a/y2014_bot3/wpilib_interface.cc
+++ b/y2014_bot3/wpilib_interface.cc
@@ -18,6 +18,7 @@
#include "frc971/wpilib/wpilib_robot_base.h"
#undef ERROR
+#include "aos/events/shm-event-loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
#include "aos/logging/queue_logging.h"
@@ -76,6 +77,9 @@
// Reads in our inputs. (sensors, voltages, etc.)
class SensorReader : public ::frc971::wpilib::SensorReader {
public:
+ SensorReader(::aos::EventLoop *event_loop)
+ : ::frc971::wpilib::SensorReader(event_loop) {}
+
void RunIteration() {
// Drivetrain
{
@@ -168,7 +172,6 @@
}
// Compressor
- ::aos::joystick_state.FetchLatest();
{
::frc971::wpilib::PneumaticsToLog to_log;
{
@@ -209,6 +212,9 @@
// Writes out rollers voltages.
class RollersWriter : public LoopOutputHandler {
public:
+ RollersWriter(::aos::EventLoop *event_loop)
+ : ::frc971::wpilib::LoopOutputHandler(event_loop) {}
+
void set_rollers_front_intake_talon(::std::unique_ptr<Talon> t_left,
::std::unique_ptr<Talon> t_right) {
rollers_front_left_intake_talon_ = ::std::move(t_left);
@@ -268,7 +274,9 @@
::aos::InitNRT();
::aos::SetCurrentThreadName("StartCompetition");
- JoystickSender joystick_sender;
+ ::aos::ShmEventLoop event_loop;
+
+ JoystickSender joystick_sender(&event_loop);
::std::thread joystick_thread(::std::ref(joystick_sender));
::frc971::wpilib::PDPFetcher pdp_fetcher;
@@ -278,23 +286,23 @@
// the robot before turning on.
// Sensors
- SensorReader reader;
+ SensorReader reader(&event_loop);
reader.set_drivetrain_left_encoder(make_encoder(4));
reader.set_drivetrain_right_encoder(make_encoder(5));
::std::thread reader_thread(::std::ref(reader));
- GyroSender gyro_sender;
+ GyroSender gyro_sender(&event_loop);
::std::thread gyro_thread(::std::ref(gyro_sender));
// Outputs
- ::frc971::wpilib::DrivetrainWriter drivetrain_writer;
+ ::frc971::wpilib::DrivetrainWriter drivetrain_writer(&event_loop);
drivetrain_writer.set_left_controller0(
::std::unique_ptr<Talon>(new Talon(5)), true);
drivetrain_writer.set_right_controller0(
::std::unique_ptr<Talon>(new Talon(2)), false);
::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
- RollersWriter rollers_writer;
+ RollersWriter rollers_writer(&event_loop);
rollers_writer.set_rollers_front_intake_talon(
::std::unique_ptr<Talon>(new Talon(3)),
::std::unique_ptr<Talon>(new Talon(7)));
diff --git a/y2016/wpilib_interface.cc b/y2016/wpilib_interface.cc
index 787d9a5..a6ea697 100644
--- a/y2016/wpilib_interface.cc
+++ b/y2016/wpilib_interface.cc
@@ -20,6 +20,7 @@
#undef ERROR
#include "aos/commonmath.h"
+#include "aos/events/shm-event-loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
#include "aos/logging/queue_logging.h"
@@ -151,7 +152,8 @@
// Class to send position messages with sensor readings to our loops.
class SensorReader : public ::frc971::wpilib::SensorReader {
public:
- SensorReader() {
+ SensorReader(::aos::EventLoop *event_loop)
+ : ::frc971::wpilib::SensorReader(event_loop) {
// Set it to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxDrivetrainShooterEncoderPulsesPerSecond);
@@ -465,6 +467,9 @@
class ShooterWriter : public ::frc971::wpilib::LoopOutputHandler {
public:
+ ShooterWriter(::aos::EventLoop *event_loop)
+ : ::frc971::wpilib::LoopOutputHandler(event_loop) {}
+
void set_shooter_left_talon(::std::unique_ptr<Talon> t) {
shooter_left_talon_ = ::std::move(t);
}
@@ -497,6 +502,9 @@
class SuperstructureWriter : public ::frc971::wpilib::LoopOutputHandler {
public:
+ SuperstructureWriter(::aos::EventLoop *event_loop)
+ : ::frc971::wpilib::LoopOutputHandler(event_loop) {}
+
void set_intake_talon(::std::unique_ptr<Talon> t) {
intake_talon_ = ::std::move(t);
}
@@ -565,12 +573,14 @@
::aos::InitNRT();
::aos::SetCurrentThreadName("StartCompetition");
- ::frc971::wpilib::JoystickSender joystick_sender;
+ ::aos::ShmEventLoop event_loop;
+
+ ::frc971::wpilib::JoystickSender joystick_sender(&event_loop);
::std::thread joystick_thread(::std::ref(joystick_sender));
::frc971::wpilib::PDPFetcher pdp_fetcher;
::std::thread pdp_fetcher_thread(::std::ref(pdp_fetcher));
- SensorReader reader;
+ SensorReader reader(&event_loop);
reader.set_drivetrain_left_encoder(make_encoder(5));
reader.set_drivetrain_right_encoder(make_encoder(6));
@@ -602,28 +612,29 @@
::std::thread reader_thread(::std::ref(reader));
// TODO(Brian): This interacts poorly with the SpiRxClearer in ADIS16448.
- ::frc971::wpilib::GyroSender gyro_sender;
+ ::frc971::wpilib::GyroSender gyro_sender(&event_loop);
::std::thread gyro_thread(::std::ref(gyro_sender));
auto imu_trigger = make_unique<DigitalInput>(3);
- ::frc971::wpilib::ADIS16448 imu(SPI::Port::kMXP, imu_trigger.get());
+ ::frc971::wpilib::ADIS16448 imu(&event_loop, SPI::Port::kMXP,
+ imu_trigger.get());
::std::thread imu_thread(::std::ref(imu));
- ::frc971::wpilib::DrivetrainWriter drivetrain_writer;
+ ::frc971::wpilib::DrivetrainWriter drivetrain_writer(&event_loop);
drivetrain_writer.set_left_controller0(
::std::unique_ptr<Talon>(new Talon(5)), false);
drivetrain_writer.set_right_controller0(
::std::unique_ptr<Talon>(new Talon(4)), true);
::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
- ShooterWriter shooter_writer;
+ ShooterWriter shooter_writer(&event_loop);
shooter_writer.set_shooter_left_talon(
::std::unique_ptr<Talon>(new Talon(9)));
shooter_writer.set_shooter_right_talon(
::std::unique_ptr<Talon>(new Talon(8)));
::std::thread shooter_writer_thread(::std::ref(shooter_writer));
- SuperstructureWriter superstructure_writer;
+ SuperstructureWriter superstructure_writer(&event_loop);
superstructure_writer.set_intake_talon(
::std::unique_ptr<Talon>(new Talon(3)));
superstructure_writer.set_shoulder_talon(
diff --git a/y2017/wpilib_interface.cc b/y2017/wpilib_interface.cc
index 12e5760..49ff30c 100644
--- a/y2017/wpilib_interface.cc
+++ b/y2017/wpilib_interface.cc
@@ -22,6 +22,7 @@
#undef ERROR
#include "aos/commonmath.h"
+#include "aos/events/shm-event-loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
#include "aos/logging/queue_logging.h"
@@ -124,7 +125,8 @@
// Class to send position messages with sensor readings to our loops.
class SensorReader : public ::frc971::wpilib::SensorReader {
public:
- SensorReader() {
+ SensorReader(::aos::EventLoop *event_loop)
+ : ::frc971::wpilib::SensorReader(event_loop) {
// Set to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
@@ -333,6 +335,9 @@
class SuperstructureWriter : public ::frc971::wpilib::LoopOutputHandler {
public:
+ SuperstructureWriter(::aos::EventLoop *event_loop)
+ : ::frc971::wpilib::LoopOutputHandler(event_loop) {}
+
void set_intake_victor(::std::unique_ptr<::frc::VictorSP> t) {
intake_victor_ = ::std::move(t);
}
@@ -436,12 +441,14 @@
::aos::InitNRT();
::aos::SetCurrentThreadName("StartCompetition");
- ::frc971::wpilib::JoystickSender joystick_sender;
+ ::aos::ShmEventLoop event_loop;
+
+ ::frc971::wpilib::JoystickSender joystick_sender(&event_loop);
::std::thread joystick_thread(::std::ref(joystick_sender));
::frc971::wpilib::PDPFetcher pdp_fetcher;
::std::thread pdp_fetcher_thread(::std::ref(pdp_fetcher));
- SensorReader reader;
+ SensorReader reader(&event_loop);
// TODO(campbell): Update port numbers
reader.set_drivetrain_left_encoder(make_encoder(0));
@@ -470,20 +477,21 @@
::std::thread reader_thread(::std::ref(reader));
auto imu_trigger = make_unique<DigitalInput>(3);
- ::frc971::wpilib::ADIS16448 imu(SPI::Port::kOnboardCS1, imu_trigger.get());
+ ::frc971::wpilib::ADIS16448 imu(&event_loop, SPI::Port::kOnboardCS1,
+ imu_trigger.get());
imu.SetDummySPI(SPI::Port::kOnboardCS2);
auto imu_reset = make_unique<DigitalOutput>(6);
imu.set_reset(imu_reset.get());
::std::thread imu_thread(::std::ref(imu));
- ::frc971::wpilib::DrivetrainWriter drivetrain_writer;
+ ::frc971::wpilib::DrivetrainWriter drivetrain_writer(&event_loop);
drivetrain_writer.set_left_controller0(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(7)), true);
drivetrain_writer.set_right_controller0(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(3)), false);
::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
- SuperstructureWriter superstructure_writer;
+ SuperstructureWriter superstructure_writer(&event_loop);
superstructure_writer.set_intake_victor(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(1)));
superstructure_writer.set_intake_rollers_victor(
diff --git a/y2018/wpilib_interface.cc b/y2018/wpilib_interface.cc
index 968de9e..6b70ef0 100644
--- a/y2018/wpilib_interface.cc
+++ b/y2018/wpilib_interface.cc
@@ -21,6 +21,7 @@
#undef ERROR
#include "aos/commonmath.h"
+#include "aos/events/shm-event-loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
#include "aos/logging/queue_logging.h"
@@ -144,7 +145,8 @@
// Class to send position messages with sensor readings to our loops.
class SensorReader : public ::frc971::wpilib::SensorReader {
public:
- SensorReader() {
+ SensorReader(::aos::EventLoop *event_loop)
+ : ::frc971::wpilib::SensorReader(event_loop) {
// Set to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
@@ -547,6 +549,9 @@
class SuperstructureWriter : public ::frc971::wpilib::LoopOutputHandler {
public:
+ SuperstructureWriter(::aos::EventLoop *event_loop)
+ : ::frc971::wpilib::LoopOutputHandler(event_loop) {}
+
void set_proximal_victor(::std::unique_ptr<::frc::VictorSP> t) {
proximal_victor_ = ::std::move(t);
}
@@ -645,12 +650,15 @@
::aos::InitNRT();
::aos::SetCurrentThreadName("StartCompetition");
- ::frc971::wpilib::JoystickSender joystick_sender;
+ ::aos::ShmEventLoop event_loop;
+
+ ::frc971::wpilib::JoystickSender joystick_sender(&event_loop);
::std::thread joystick_thread(::std::ref(joystick_sender));
::frc971::wpilib::PDPFetcher pdp_fetcher;
::std::thread pdp_fetcher_thread(::std::ref(pdp_fetcher));
- SensorReader reader;
+
+ SensorReader reader(&event_loop);
// TODO(Sabina): Update port numbers(Sensors and Victors)
reader.set_drivetrain_left_encoder(make_encoder(0));
@@ -690,7 +698,7 @@
::std::thread reader_thread(::std::ref(reader));
auto imu_trigger = make_unique<frc::DigitalInput>(5);
- ::frc971::wpilib::ADIS16448 imu(frc::SPI::Port::kOnboardCS1,
+ ::frc971::wpilib::ADIS16448 imu(&event_loop, frc::SPI::Port::kOnboardCS1,
imu_trigger.get());
imu.SetDummySPI(frc::SPI::Port::kOnboardCS2);
auto imu_reset = make_unique<frc::DigitalOutput>(6);
@@ -701,14 +709,14 @@
// they are identical, as far as DrivetrainWriter is concerned, to the SP
// variety so all the Victors are written as SPs.
- ::frc971::wpilib::DrivetrainWriter drivetrain_writer;
+ ::frc971::wpilib::DrivetrainWriter drivetrain_writer(&event_loop);
drivetrain_writer.set_left_controller0(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(2)), false);
drivetrain_writer.set_right_controller0(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(3)), true);
::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
- SuperstructureWriter superstructure_writer;
+ SuperstructureWriter superstructure_writer(&event_loop);
superstructure_writer.set_left_intake_elastic_victor(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(4)));
superstructure_writer.set_left_intake_rollers_victor(
diff --git a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
index 1acdb66..75a8af3 100644
--- a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
+++ b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
@@ -51,7 +51,7 @@
camera_queue_(::y2019::control_loops::drivetrain::camera_frames.name()),
localizer_(dt_config_, &event_loop_),
drivetrain_motor_(dt_config_, &event_loop_, &localizer_),
- drivetrain_motor_plant_(dt_config_),
+ drivetrain_motor_plant_(&simulation_event_loop_, dt_config_),
cameras_(MakeCameras(&robot_pose_)),
last_frame_(monotonic_clock::now()) {
set_team_id(::frc971::control_loops::testing::kTeamNumber);
@@ -177,6 +177,7 @@
::frc971::control_loops::DrivetrainQueue my_drivetrain_queue_;
::aos::Queue<CameraFrame> camera_queue_;
::aos::ShmEventLoop event_loop_;
+ ::aos::ShmEventLoop simulation_event_loop_;
EventLoopLocalizer localizer_;
DrivetrainLoop drivetrain_motor_;
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 54c2ebc..29f110a 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -20,6 +20,7 @@
#undef ERROR
#include "aos/commonmath.h"
+#include "aos/events/shm-event-loop.h"
#include "aos/init.h"
#include "aos/logging/logging.h"
#include "aos/logging/queue_logging.h"
@@ -128,7 +129,8 @@
// Class to send position messages with sensor readings to our loops.
class SensorReader : public ::frc971::wpilib::SensorReader {
public:
- SensorReader() {
+ SensorReader(::aos::EventLoop *event_loop)
+ : ::frc971::wpilib::SensorReader(event_loop) {
// Set to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
@@ -429,6 +431,11 @@
class SuperstructureWriter : public ::frc971::wpilib::LoopOutputHandler {
public:
+ SuperstructureWriter(::aos::EventLoop *event_loop)
+ : ::frc971::wpilib::LoopOutputHandler(event_loop),
+ robot_state_fetcher_(
+ event_loop->MakeFetcher<::aos::RobotState>(".aos.robot_state")) {}
+
void set_elevator_victor(::std::unique_ptr<::frc::VictorSP> t) {
elevator_victor_ = ::std::move(t);
}
@@ -476,9 +483,10 @@
-kMaxBringupPower, kMaxBringupPower) /
12.0);
- ::aos::robot_state.FetchLatest();
- const double battery_voltage =
- ::aos::robot_state.get() ? ::aos::robot_state->voltage_battery : 12.0;
+ robot_state_fetcher_.Fetch();
+ const double battery_voltage = robot_state_fetcher_.get()
+ ? robot_state_fetcher_->voltage_battery
+ : 12.0;
// Throw a fast low pass filter on the battery voltage so we don't respond
// too fast to noise.
@@ -499,6 +507,8 @@
suction_victor_->SetDisabled();
}
+ ::aos::Fetcher<::aos::RobotState> robot_state_fetcher_;
+
::std::unique_ptr<::frc::VictorSP> elevator_victor_, intake_victor_,
wrist_victor_, stilts_victor_, suction_victor_;
@@ -661,12 +671,14 @@
::aos::InitNRT();
::aos::SetCurrentThreadName("StartCompetition");
- ::frc971::wpilib::JoystickSender joystick_sender;
+ ::aos::ShmEventLoop event_loop;
+
+ ::frc971::wpilib::JoystickSender joystick_sender(&event_loop);
::std::thread joystick_thread(::std::ref(joystick_sender));
::frc971::wpilib::PDPFetcher pdp_fetcher;
::std::thread pdp_fetcher_thread(::std::ref(pdp_fetcher));
- SensorReader reader;
+ SensorReader reader(&event_loop);
reader.set_drivetrain_left_encoder(make_encoder(0));
reader.set_drivetrain_right_encoder(make_encoder(1));
@@ -706,7 +718,7 @@
camera_reader.set_activate_passthrough(make_unique<frc::DigitalInput>(25));
auto imu_trigger = make_unique<frc::DigitalInput>(0);
- ::frc971::wpilib::ADIS16448 imu(frc::SPI::Port::kOnboardCS1,
+ ::frc971::wpilib::ADIS16448 imu(&event_loop, frc::SPI::Port::kOnboardCS1,
imu_trigger.get());
imu.set_spi_idle_callback(
[&camera_reader]() { camera_reader.DoSpiTransaction(); });
@@ -718,14 +730,14 @@
// they are identical, as far as DrivetrainWriter is concerned, to the SP
// variety so all the Victors are written as SPs.
- ::frc971::wpilib::DrivetrainWriter drivetrain_writer;
+ ::frc971::wpilib::DrivetrainWriter drivetrain_writer(&event_loop);
drivetrain_writer.set_left_controller0(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(0)), true);
drivetrain_writer.set_right_controller0(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(1)), false);
::std::thread drivetrain_writer_thread(::std::ref(drivetrain_writer));
- SuperstructureWriter superstructure_writer;
+ SuperstructureWriter superstructure_writer(&event_loop);
superstructure_writer.set_elevator_victor(
::std::unique_ptr<::frc::VictorSP>(new ::frc::VictorSP(4)));
// TODO(austin): Do the vacuum