Remove global .frc971.control_loops.drivetrain_queue object
Change-Id: I424f09dcc8bc210e49cbdc805d1a423a72332617
diff --git a/y2016/actors/autonomous_actor.cc b/y2016/actors/autonomous_actor.cc
index e94cccd..01650c4 100644
--- a/y2016/actors/autonomous_actor.cc
+++ b/y2016/actors/autonomous_actor.cc
@@ -18,7 +18,6 @@
namespace y2016 {
namespace actors {
using ::aos::monotonic_clock;
-using ::frc971::control_loops::drivetrain_queue;
namespace chrono = ::std::chrono;
namespace this_thread = ::std::this_thread;
@@ -245,20 +244,20 @@
last_angle = vision_status_fetcher_->horizontal_angle;
}
- drivetrain_queue.status.FetchLatest();
- drivetrain_queue.goal.FetchLatest();
+ drivetrain_status_fetcher_.Fetch();
+ drivetrain_goal_fetcher_.Fetch();
- if (drivetrain_queue.status.get() && drivetrain_queue.goal.get()) {
- const double left_goal = drivetrain_queue.goal->left_goal;
- const double right_goal = drivetrain_queue.goal->right_goal;
+ if (drivetrain_status_fetcher_.get() && drivetrain_goal_fetcher_.get()) {
+ const double left_goal = drivetrain_goal_fetcher_->left_goal;
+ const double right_goal = drivetrain_goal_fetcher_->right_goal;
const double left_current =
- drivetrain_queue.status->estimated_left_position;
+ drivetrain_status_fetcher_->estimated_left_position;
const double right_current =
- drivetrain_queue.status->estimated_right_position;
+ drivetrain_status_fetcher_->estimated_right_position;
const double left_velocity =
- drivetrain_queue.status->estimated_left_velocity;
+ drivetrain_status_fetcher_->estimated_left_velocity;
const double right_velocity =
- drivetrain_queue.status->estimated_right_velocity;
+ drivetrain_status_fetcher_->estimated_right_velocity;
if (vision_valid && ::std::abs(last_angle) < 0.02 &&
::std::abs((left_goal - right_goal) -
@@ -496,14 +495,14 @@
if (!WaitForAboveAngle(above)) return;
// Ok, we are good now. Compensate by moving the goal by the error.
// Should be here at 2.7
- drivetrain_queue.status.FetchLatest();
- if (drivetrain_queue.status.get()) {
+ drivetrain_status_fetcher_.Fetch();
+ if (drivetrain_status_fetcher_.get()) {
const double left_error =
(initial_drivetrain_.left -
- drivetrain_queue.status->estimated_left_position);
+ drivetrain_status_fetcher_->estimated_left_position);
const double right_error =
(initial_drivetrain_.right -
- drivetrain_queue.status->estimated_right_position);
+ drivetrain_status_fetcher_->estimated_right_position);
const double distance_to_go = (left_error + right_error) / 2.0;
const double distance_compensation =
goal_distance - tip_distance - distance_to_go;
@@ -558,7 +557,7 @@
return;
}
phased_loop.SleepUntilNext();
- drivetrain_queue.status.FetchLatest();
+ drivetrain_status_fetcher_.Fetch();
if (IsDriveDone()) {
return;
}
diff --git a/y2016/actors/vision_align_actor.cc b/y2016/actors/vision_align_actor.cc
index 1110f92..f42aad9 100644
--- a/y2016/actors/vision_align_actor.cc
+++ b/y2016/actors/vision_align_actor.cc
@@ -20,14 +20,17 @@
namespace y2016 {
namespace actors {
-using ::frc971::control_loops::drivetrain_queue;
VisionAlignActor::VisionAlignActor(::aos::EventLoop *event_loop)
: aos::common::actions::ActorBase<actors::VisionAlignActionQueueGroup>(
event_loop, ".y2016.actors.vision_align_action"),
vision_status_fetcher_(
event_loop->MakeFetcher<::y2016::vision::VisionStatus>(
- ".y2016.vision.vision_status")) {}
+ ".y2016.vision.vision_status")),
+ drivetrain_goal_sender_(
+ event_loop
+ ->MakeSender<::frc971::control_loops::DrivetrainQueue::Goal>(
+ ".frc971.control_loops.drivetrain_queue.goal")) {}
bool VisionAlignActor::RunAction(
const actors::VisionAlignActionParams & /*params*/) {
@@ -64,15 +67,16 @@
const double left_current = vision_status.drivetrain_left_position;
const double right_current = vision_status.drivetrain_right_position;
- if (!drivetrain_queue.goal.MakeWithBuilder()
- .wheel(0.0)
- .throttle(0.0)
- .highgear(false)
- .quickturn(false)
- .controller_type(1)
- .left_goal(left_current + side_distance_change)
- .right_goal(right_current - side_distance_change)
- .Send()) {
+ auto drivetrain_message = drivetrain_goal_sender_.MakeMessage();
+ drivetrain_message->wheel = 0.0;
+ drivetrain_message->throttle = 0.0;
+ drivetrain_message->highgear = false;
+ drivetrain_message->quickturn = false;
+ drivetrain_message->controller_type = 1;
+ drivetrain_message->left_goal = left_current + side_distance_change;
+ drivetrain_message->right_goal = right_current - side_distance_change;
+
+ if (!drivetrain_message.Send()) {
LOG(WARNING, "sending drivetrain goal failed\n");
}
}
diff --git a/y2016/actors/vision_align_actor.h b/y2016/actors/vision_align_actor.h
index f92f507..586f7d0 100644
--- a/y2016/actors/vision_align_actor.h
+++ b/y2016/actors/vision_align_actor.h
@@ -5,6 +5,7 @@
#include "aos/actions/actions.h"
#include "aos/actions/actor.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "y2016/actors/vision_align_action.q.h"
#include "y2016/vision/vision.q.h"
@@ -29,6 +30,8 @@
private:
::aos::Fetcher<::y2016::vision::VisionStatus> vision_status_fetcher_;
+ ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Goal>
+ drivetrain_goal_sender_;
};
} // namespace actors
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index 618b538..64c033a 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -25,8 +25,6 @@
#include "y2016/queues/ball_detector.q.h"
#include "y2016/vision/vision.q.h"
-using ::frc971::control_loops::drivetrain_queue;
-
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::ControlBit;
using ::aos::input::driver_station::JoystickAxis;
@@ -91,6 +89,14 @@
event_loop
->MakeSender<::y2016::control_loops::SuperstructureQueue::Goal>(
".y2016.control_loops.superstructure_queue.goal")),
+ drivetrain_goal_fetcher_(
+ event_loop
+ ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Goal>(
+ ".frc971.control_loops.drivetrain_queue.goal")),
+ drivetrain_status_fetcher_(
+ event_loop
+ ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
+ ".frc971.control_loops.drivetrain_queue.status")),
intake_goal_(0.0),
shoulder_goal_(M_PI / 2.0),
wrist_goal_(0.0),
@@ -183,8 +189,9 @@
// Forwards shot
shoulder_goal_ = M_PI / 2.0 + 0.1;
wrist_goal_ = M_PI + 0.41 + 0.02 - 0.005;
- if (drivetrain_queue.status.get()) {
- wrist_goal_ += drivetrain_queue.status->ground_angle;
+ drivetrain_status_fetcher_.Fetch();
+ if (drivetrain_status_fetcher_.get()) {
+ wrist_goal_ += drivetrain_status_fetcher_->ground_angle;
}
shooter_velocity_ = 640.0;
intake_goal_ = intake_when_shooting;
@@ -192,8 +199,9 @@
// Backwards shot
shoulder_goal_ = M_PI / 2.0 - 0.4;
wrist_goal_ = -0.62 - 0.02;
- if (drivetrain_queue.status.get()) {
- wrist_goal_ += drivetrain_queue.status->ground_angle;
+ drivetrain_status_fetcher_.Fetch();
+ if (drivetrain_status_fetcher_.get()) {
+ wrist_goal_ += drivetrain_status_fetcher_->ground_angle;
}
shooter_velocity_ = 640.0;
intake_goal_ = intake_when_shooting;
@@ -259,19 +267,20 @@
if (data.IsPressed(kFire) && shooter_velocity_ != 0.0) {
if (data.IsPressed(kVisionAlign)) {
// Make sure that we are lined up.
- drivetrain_queue.status.FetchLatest();
- drivetrain_queue.goal.FetchLatest();
- if (drivetrain_queue.status.get() && drivetrain_queue.goal.get()) {
- const double left_goal = drivetrain_queue.goal->left_goal;
- const double right_goal = drivetrain_queue.goal->right_goal;
+ drivetrain_status_fetcher_.Fetch();
+ drivetrain_goal_fetcher_.Fetch();
+ if (drivetrain_status_fetcher_.get() &&
+ drivetrain_goal_fetcher_.get()) {
+ const double left_goal = drivetrain_goal_fetcher_->left_goal;
+ const double right_goal = drivetrain_goal_fetcher_->right_goal;
const double left_current =
- drivetrain_queue.status->estimated_left_position;
+ drivetrain_status_fetcher_->estimated_left_position;
const double right_current =
- drivetrain_queue.status->estimated_right_position;
+ drivetrain_status_fetcher_->estimated_right_position;
const double left_velocity =
- drivetrain_queue.status->estimated_left_velocity;
+ drivetrain_status_fetcher_->estimated_left_velocity;
const double right_velocity =
- drivetrain_queue.status->estimated_right_velocity;
+ drivetrain_status_fetcher_->estimated_right_velocity;
if (vision_action_running_ && ::std::abs(last_angle_) < 0.02 &&
::std::abs((left_goal - right_goal) -
(left_current - right_current)) /
@@ -399,6 +408,10 @@
superstructure_status_fetcher_;
::aos::Sender<::y2016::control_loops::SuperstructureQueue::Goal>
superstructure_goal_sender_;
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Goal>
+ drivetrain_goal_fetcher_;
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ drivetrain_status_fetcher_;
// Whatever these are set to are our default goals to send out after zeroing.
double intake_goal_;
diff --git a/y2016/vision/target_receiver.cc b/y2016/vision/target_receiver.cc
index 57fbd51..a496848 100644
--- a/y2016/vision/target_receiver.cc
+++ b/y2016/vision/target_receiver.cc
@@ -194,26 +194,25 @@
// Handles calculating drivetrain offsets.
class DrivetrainOffsetCalculator {
public:
- // To be called by ::std::thread.
- void operator()() {
- auto &status = ::frc971::control_loops::drivetrain_queue.status;
- while (run_) {
- status.FetchAnother();
-
- ::aos::MutexLocker locker(&lock_);
- data_[data_index_].time = status->sent_time;
- data_[data_index_].left = status->estimated_left_position;
- data_[data_index_].right = status->estimated_right_position;
- ++data_index_;
- if (data_index_ == data_.size()) data_index_ = 0;
- if (valid_data_ < data_.size()) ++valid_data_;
- }
- }
+ DrivetrainOffsetCalculator(::aos::EventLoop *event_loop)
+ : drivetrain_status_fetcher_(
+ event_loop
+ ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
+ ".frc971.control_loops.drivetrain_queue.status")) {}
// Takes a vision status message with everything except
// drivetrain_{left,right}_position set and sets those.
// Returns false if it doesn't have enough data to fill them out.
bool CompleteVisionStatus(::y2016::vision::VisionStatus *status) {
+ while (drivetrain_status_fetcher_.FetchNext()) {
+ data_[data_index_].time = drivetrain_status_fetcher_->sent_time;
+ data_[data_index_].left = drivetrain_status_fetcher_->estimated_left_position;
+ data_[data_index_].right = drivetrain_status_fetcher_->estimated_right_position;
+ ++data_index_;
+ if (data_index_ == data_.size()) data_index_ = 0;
+ if (valid_data_ < data_.size()) ++valid_data_;
+ }
+
if (valid_data_ == 0) return false;
const monotonic_clock::time_point capture_time =
@@ -237,8 +236,6 @@
return true;
}
- void Quit() { run_ = false; }
-
private:
struct DrivetrainData {
monotonic_clock::time_point time;
@@ -250,7 +247,6 @@
// Do not call this if valid_data_ is 0.
void FindBeforeAfter(DrivetrainData *before, DrivetrainData *after,
monotonic_clock::time_point capture_time) {
- ::aos::MutexLocker locker(&lock_);
size_t location = 0;
while (true) {
// We hit the end of our data. Just fill them both out as the last data
@@ -290,15 +286,14 @@
}
}
+ ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ drivetrain_status_fetcher_;
+
::std::array<DrivetrainData, 200> data_;
// The index into data_ the next data point is going at.
size_t data_index_ = 0;
// How many elemets of data_ are valid.
size_t valid_data_ = 0;
-
- ::aos::Mutex lock_;
-
- ::std::atomic<bool> run_{true};
};
void Main() {
@@ -312,8 +307,7 @@
LOG(INFO, "calibration: %s\n",
stereo.calibration().ShortDebugString().c_str());
- DrivetrainOffsetCalculator drivetrain_offset;
- ::std::thread drivetrain_offset_thread(::std::ref(drivetrain_offset));
+ DrivetrainOffsetCalculator drivetrain_offset(&event_loop);
CameraHandler left;
CameraHandler right;
@@ -426,9 +420,6 @@
right.target().ShortDebugString().c_str());
}
}
-
- drivetrain_offset.Quit();
- drivetrain_offset_thread.join();
}
} // namespace vision
diff --git a/y2016/wpilib_interface.cc b/y2016/wpilib_interface.cc
index c2a4271..5b5fecb 100644
--- a/y2016/wpilib_interface.cc
+++ b/y2016/wpilib_interface.cc
@@ -53,7 +53,6 @@
#include "y2016/control_loops/superstructure/superstructure.q.h"
#include "y2016/queues/ball_detector.q.h"
-using ::frc971::control_loops::drivetrain_queue;
using aos::make_unique;
using ::frc971::wpilib::LoopOutputHandler;
using ::y2016::control_loops::shooter::ShooterQueue;
@@ -158,7 +157,11 @@
".y2016.control_loops.shooter.shooter_queue.position")),
superstructure_position_sender_(
event_loop->MakeSender<SuperstructureQueue::Position>(
- ".y2016.control_loops.superstructure_queue.position")) {
+ ".y2016.control_loops.superstructure_queue.position")),
+ drivetrain_position_sender_(
+ event_loop->MakeSender<
+ ::frc971::control_loops::DrivetrainQueue::Position>(
+ ".frc971.control_loops.drivetrain_queue.position")) {
// Set it to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxDrivetrainShooterEncoderPulsesPerSecond);
@@ -255,7 +258,7 @@
void RunIteration() {
{
- auto drivetrain_message = drivetrain_queue.position.MakeMessage();
+ auto drivetrain_message = drivetrain_position_sender_.MakeMessage();
drivetrain_message->right_encoder =
drivetrain_translate(-drivetrain_right_encoder_->GetRaw());
drivetrain_message->left_encoder =
@@ -326,6 +329,8 @@
::aos::Sender<::frc971::autonomous::AutonomousMode> auto_mode_sender_;
::aos::Sender<ShooterQueue::Position> shooter_position_sender_;
::aos::Sender<SuperstructureQueue::Position> superstructure_position_sender_;
+ ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Position>
+ drivetrain_position_sender_;
::std::unique_ptr<::frc::AnalogInput> drivetrain_left_hall_,
drivetrain_right_hall_;