cleaned up queue wrapper stuff
diff --git a/aos/common/queue.h b/aos/common/queue.h
index 6c33822..927f11f 100644
--- a/aos/common/queue.h
+++ b/aos/common/queue.h
@@ -213,15 +213,19 @@
// Fetches the next message from the queue.
// Returns true if there was a new message available and we successfully
- // fetched it. This removes the message from the queue for all readers.
+ // fetched it.
bool FetchNext();
- bool FetchNextBlocking();
+ void FetchNextBlocking();
// Fetches the last message from the queue.
// Returns true if there was a new message available and we successfully
// fetched it.
bool FetchLatest();
+ // Fetches another message from the queue. Blocks until there is one if the
+ // latest was already Fetched.
+ void FetchAnother();
+
// Returns the age of the message.
const time::Time Age() { return time::Time::Now() - queue_msg_->sent_time; }
diff --git a/aos/common/queue_test.cc b/aos/common/queue_test.cc
index 58c88dc..dfafe6a 100644
--- a/aos/common/queue_test.cc
+++ b/aos/common/queue_test.cc
@@ -34,7 +34,7 @@
MyThread() : threaded_test_queue(".aos.common.testing.test_queue") {}
virtual void Run() {
- ASSERT_TRUE(threaded_test_queue.FetchNextBlocking());
+ threaded_test_queue.FetchNextBlocking();
EXPECT_TRUE(threaded_test_queue->test_bool);
EXPECT_EQ(0x971, threaded_test_queue->test_int);
}
diff --git a/aos/linux_code/queue-tmpl.h b/aos/linux_code/queue-tmpl.h
index 15029986..64121f3 100644
--- a/aos/linux_code/queue-tmpl.h
+++ b/aos/linux_code/queue-tmpl.h
@@ -193,12 +193,12 @@
}
template <class T>
-bool Queue<T>::FetchNextBlocking() {
+void Queue<T>::FetchNextBlocking() {
Init();
- const T *msg = static_cast<const T *>(queue_->ReadMessageIndex(RawQueue::kBlock, &index_));
+ const T *msg = static_cast<const T *>(
+ queue_->ReadMessageIndex(RawQueue::kBlock, &index_));
queue_msg_.reset(msg);
assert (msg != NULL);
- return true;
}
template <class T>
@@ -218,6 +218,11 @@
}
template <class T>
+void Queue<T>::FetchAnother() {
+ if (!FetchLatest()) FetchNextBlocking();
+}
+
+template <class T>
SafeScopedMessagePtr<T> Queue<T>::SafeMakeMessage() {
Init();
SafeScopedMessagePtr<T> safe_msg(queue_);
diff --git a/frc971/actions/shoot_action.cc b/frc971/actions/shoot_action.cc
index e302912..211d81f 100644
--- a/frc971/actions/shoot_action.cc
+++ b/frc971/actions/shoot_action.cc
@@ -10,6 +10,31 @@
namespace frc971 {
namespace actions {
+namespace {
+
+bool IntakeOff() {
+ control_loops::claw_queue_group.goal.FetchLatest();
+ if (!control_loops::claw_queue_group.goal.get()) {
+ LOG(WARNING, "no claw goal\n");
+ // If it doesn't have a goal, then the intake isn't on so we don't have to
+ // turn it off.
+ return true;
+ } else {
+ if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
+ .bottom_angle(control_loops::claw_queue_group.goal->bottom_angle)
+ .separation_angle(
+ control_loops::claw_queue_group.goal->separation_angle)
+ .intake(0.0)
+ .centering(0.0)
+ .Send()) {
+ LOG(WARNING, "sending claw goal failed\n");
+ return false;
+ }
+ }
+ return true;
+}
+
+} // namespace
constexpr double ShootAction::kOffsetRadians;
constexpr double ShootAction::kClawShootingSeparation;
@@ -53,19 +78,7 @@
return;
}
- // Turn the intake off.
- control_loops::claw_queue_group.goal.FetchLatest();
-
- if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
- .bottom_angle(control_loops::claw_queue_group.goal->bottom_angle)
- .separation_angle(
- control_loops::claw_queue_group.goal->separation_angle)
- .intake(0.0)
- .centering(0.0)
- .Send()) {
- LOG(WARNING, "sending claw goal failed\n");
- return;
- }
+ if (!IntakeOff()) return;
// Make sure that we have the latest shooter status.
control_loops::shooter_queue_group.status.FetchLatest();
@@ -86,19 +99,7 @@
// wait for record of shot having been fired
if (WaitUntil(::std::bind(&ShootAction::DoneShot, this))) return;
- // Turn the intake off.
- control_loops::claw_queue_group.goal.FetchLatest();
-
- if (!control_loops::claw_queue_group.goal.MakeWithBuilder()
- .bottom_angle(control_loops::claw_queue_group.goal->bottom_angle)
- .separation_angle(
- control_loops::claw_queue_group.goal->separation_angle)
- .intake(0.0)
- .centering(0.0)
- .Send()) {
- LOG(WARNING, "sending claw goal failed\n");
- return;
- }
+ if (!IntakeOff()) return;
}
bool ClawIsReady() {
@@ -146,12 +147,8 @@
}
bool ShootAction::DoneSetupShot() {
- if (!control_loops::shooter_queue_group.status.FetchLatest()) {
- control_loops::shooter_queue_group.status.FetchNextBlocking();
- }
- if (!control_loops::claw_queue_group.status.FetchLatest()) {
- control_loops::claw_queue_group.status.FetchNextBlocking();
- }
+ control_loops::shooter_queue_group.status.FetchAnother();
+ control_loops::claw_queue_group.status.FetchAnother();
// Make sure that both the shooter and claw have reached the necessary
// states.
if (ShooterIsReady() && ClawIsReady()) {
@@ -163,9 +160,7 @@
}
bool ShootAction::DonePreShotOpen() {
- if (!control_loops::claw_queue_group.status.FetchLatest()) {
- control_loops::claw_queue_group.status.FetchNextBlocking();
- }
+ control_loops::claw_queue_group.status.FetchAnother();
if (control_loops::claw_queue_group.status->separation >
kClawShootingSeparation) {
LOG(INFO, "Opened up enough to shoot.\n");
@@ -175,9 +170,7 @@
}
bool ShootAction::DoneShot() {
- if (!control_loops::shooter_queue_group.status.FetchLatest()) {
- control_loops::shooter_queue_group.status.FetchNextBlocking();
- }
+ control_loops::shooter_queue_group.status.FetchAnother();
if (control_loops::shooter_queue_group.status->shots > previous_shots_) {
LOG(INFO, "Shot succeeded!\n");
return true;
diff --git a/frc971/control_loops/claw/claw_calibration.cc b/frc971/control_loops/claw/claw_calibration.cc
index 608c72d..94bff15 100644
--- a/frc971/control_loops/claw/claw_calibration.cc
+++ b/frc971/control_loops/claw/claw_calibration.cc
@@ -148,7 +148,7 @@
};
int Main() {
- while (!control_loops::claw_queue_group.position.FetchNextBlocking());
+ control_loops::claw_queue_group.position.FetchNextBlocking();
const double top_start_position =
control_loops::claw_queue_group.position->top.position;
@@ -216,87 +216,86 @@
*control_loops::claw_queue_group.position;
while (true) {
- if (control_loops::claw_queue_group.position.FetchNextBlocking()) {
- bool print = false;
- if (top.GetPositionOfEdge(control_loops::claw_queue_group.position->top,
- &limits.upper_claw)) {
- print = true;
- printf("Got an edge on the upper claw\n");
- }
- if (bottom.GetPositionOfEdge(
- control_loops::claw_queue_group.position->bottom,
- &limits.lower_claw)) {
- print = true;
- printf("Got an edge on the lower claw\n");
- }
- const double top_position =
- control_loops::claw_queue_group.position->top.position -
- top_start_position;
- const double bottom_position =
- control_loops::claw_queue_group.position->bottom.position -
- bottom_start_position;
- const double separation = top_position - bottom_position;
- if (separation > limits.claw_max_separation) {
- limits.soft_max_separation = limits.claw_max_separation = separation;
- print = true;
- }
- if (separation < limits.claw_min_separation) {
- limits.soft_min_separation = limits.claw_min_separation = separation;
- print = true;
- }
-
- if (print) {
- printf("{%f,\n", limits.claw_zeroing_off_speed);
- printf("%f,\n", limits.claw_zeroing_speed);
- printf("%f,\n", limits.claw_zeroing_separation);
- printf("%f,\n", limits.claw_min_separation);
- printf("%f,\n", limits.claw_max_separation);
- printf("%f,\n", limits.soft_min_separation);
- printf("%f,\n", limits.soft_max_separation);
- printf(
- "{%f, %f, %f, %f, {%f, %f, %f, %f}, {%f, %f, %f, %f}, {%f, %f, %f, "
- "%f}},\n",
- limits.upper_claw.lower_hard_limit,
- limits.upper_claw.upper_hard_limit, limits.upper_claw.lower_limit,
- limits.upper_claw.upper_limit, limits.upper_claw.front.lower_angle,
- limits.upper_claw.front.upper_angle,
- limits.upper_claw.front.lower_decreasing_angle,
- limits.upper_claw.front.upper_decreasing_angle,
- limits.upper_claw.calibration.lower_angle,
- limits.upper_claw.calibration.upper_angle,
- limits.upper_claw.calibration.lower_decreasing_angle,
- limits.upper_claw.calibration.upper_decreasing_angle,
- limits.upper_claw.back.lower_angle,
- limits.upper_claw.back.upper_angle,
- limits.upper_claw.back.lower_decreasing_angle,
- limits.upper_claw.back.upper_decreasing_angle);
-
- printf(
- "{%f, %f, %f, %f, {%f, %f, %f, %f}, {%f, %f, %f, %f}, {%f, %f, %f, "
- "%f}},\n",
- limits.lower_claw.lower_hard_limit,
- limits.lower_claw.upper_hard_limit, limits.lower_claw.lower_limit,
- limits.lower_claw.upper_limit, limits.lower_claw.front.lower_angle,
- limits.lower_claw.front.upper_angle,
- limits.lower_claw.front.lower_decreasing_angle,
- limits.lower_claw.front.upper_decreasing_angle,
- limits.lower_claw.calibration.lower_angle,
- limits.lower_claw.calibration.upper_angle,
- limits.lower_claw.calibration.lower_decreasing_angle,
- limits.lower_claw.calibration.upper_decreasing_angle,
- limits.lower_claw.back.lower_angle,
- limits.lower_claw.back.upper_angle,
- limits.lower_claw.back.lower_decreasing_angle,
- limits.lower_claw.back.upper_decreasing_angle);
- printf("%f, // claw_unimportant_epsilon\n",
- limits.claw_unimportant_epsilon);
- printf("%f, // start_fine_tune_pos\n", limits.start_fine_tune_pos);
- printf("%f,\n", limits.max_zeroing_voltage);
- printf("}\n");
- }
-
- last_position = *control_loops::claw_queue_group.position;
+ control_loops::claw_queue_group.position.FetchNextBlocking();
+ bool print = false;
+ if (top.GetPositionOfEdge(control_loops::claw_queue_group.position->top,
+ &limits.upper_claw)) {
+ print = true;
+ printf("Got an edge on the upper claw\n");
}
+ if (bottom.GetPositionOfEdge(
+ control_loops::claw_queue_group.position->bottom,
+ &limits.lower_claw)) {
+ print = true;
+ printf("Got an edge on the lower claw\n");
+ }
+ const double top_position =
+ control_loops::claw_queue_group.position->top.position -
+ top_start_position;
+ const double bottom_position =
+ control_loops::claw_queue_group.position->bottom.position -
+ bottom_start_position;
+ const double separation = top_position - bottom_position;
+ if (separation > limits.claw_max_separation) {
+ limits.soft_max_separation = limits.claw_max_separation = separation;
+ print = true;
+ }
+ if (separation < limits.claw_min_separation) {
+ limits.soft_min_separation = limits.claw_min_separation = separation;
+ print = true;
+ }
+
+ if (print) {
+ printf("{%f,\n", limits.claw_zeroing_off_speed);
+ printf("%f,\n", limits.claw_zeroing_speed);
+ printf("%f,\n", limits.claw_zeroing_separation);
+ printf("%f,\n", limits.claw_min_separation);
+ printf("%f,\n", limits.claw_max_separation);
+ printf("%f,\n", limits.soft_min_separation);
+ printf("%f,\n", limits.soft_max_separation);
+ printf(
+ "{%f, %f, %f, %f, {%f, %f, %f, %f}, {%f, %f, %f, %f}, {%f, %f, %f, "
+ "%f}},\n",
+ limits.upper_claw.lower_hard_limit,
+ limits.upper_claw.upper_hard_limit, limits.upper_claw.lower_limit,
+ limits.upper_claw.upper_limit, limits.upper_claw.front.lower_angle,
+ limits.upper_claw.front.upper_angle,
+ limits.upper_claw.front.lower_decreasing_angle,
+ limits.upper_claw.front.upper_decreasing_angle,
+ limits.upper_claw.calibration.lower_angle,
+ limits.upper_claw.calibration.upper_angle,
+ limits.upper_claw.calibration.lower_decreasing_angle,
+ limits.upper_claw.calibration.upper_decreasing_angle,
+ limits.upper_claw.back.lower_angle,
+ limits.upper_claw.back.upper_angle,
+ limits.upper_claw.back.lower_decreasing_angle,
+ limits.upper_claw.back.upper_decreasing_angle);
+
+ printf(
+ "{%f, %f, %f, %f, {%f, %f, %f, %f}, {%f, %f, %f, %f}, {%f, %f, %f, "
+ "%f}},\n",
+ limits.lower_claw.lower_hard_limit,
+ limits.lower_claw.upper_hard_limit, limits.lower_claw.lower_limit,
+ limits.lower_claw.upper_limit, limits.lower_claw.front.lower_angle,
+ limits.lower_claw.front.upper_angle,
+ limits.lower_claw.front.lower_decreasing_angle,
+ limits.lower_claw.front.upper_decreasing_angle,
+ limits.lower_claw.calibration.lower_angle,
+ limits.lower_claw.calibration.upper_angle,
+ limits.lower_claw.calibration.lower_decreasing_angle,
+ limits.lower_claw.calibration.upper_decreasing_angle,
+ limits.lower_claw.back.lower_angle,
+ limits.lower_claw.back.upper_angle,
+ limits.lower_claw.back.lower_decreasing_angle,
+ limits.lower_claw.back.upper_decreasing_angle);
+ printf("%f, // claw_unimportant_epsilon\n",
+ limits.claw_unimportant_epsilon);
+ printf("%f, // start_fine_tune_pos\n", limits.start_fine_tune_pos);
+ printf("%f,\n", limits.max_zeroing_voltage);
+ printf("}\n");
+ }
+
+ last_position = *control_loops::claw_queue_group.position;
}
return 0;
}
diff --git a/frc971/output/led_setter.cc b/frc971/output/led_setter.cc
index a69183d..53c4974 100644
--- a/frc971/output/led_setter.cc
+++ b/frc971/output/led_setter.cc
@@ -13,7 +13,7 @@
::bbb::LED claw_zeroed(3);
while (true) {
- CHECK(claw_queue_group.status.FetchNextBlocking());
+ claw_queue_group.status.FetchNextBlocking();
claw_zeroed.Set(claw_queue_group.status->zeroed_for_auto);
}
}