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