Convert aos over to flatbuffers
Everything builds, and all the tests pass. I suspect that some entries
are missing from the config files, but those will be found pretty
quickly on startup.
There is no logging or live introspection of queue messages.
Change-Id: I496ee01ed68f202c7851bed7e8786cee30df29f5
diff --git a/y2014/control_loops/claw/BUILD b/y2014/control_loops/claw/BUILD
index 6c162e6..670dd76 100644
--- a/y2014/control_loops/claw/BUILD
+++ b/y2014/control_loops/claw/BUILD
@@ -1,16 +1,40 @@
package(default_visibility = ["//visibility:public"])
-load("//aos/build:queues.bzl", "queue_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
-queue_library(
- name = "claw_queue",
+flatbuffer_cc_library(
+ name = "claw_goal_fbs",
srcs = [
- "claw.q",
+ "claw_goal.fbs",
],
- deps = [
- "//aos/controls:control_loop_queues",
- "//frc971/control_loops:queues",
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "claw_position_fbs",
+ srcs = [
+ "claw_position.fbs",
],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "claw_output_fbs",
+ srcs = [
+ "claw_output.fbs",
+ ],
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "claw_status_fbs",
+ srcs = [
+ "claw_status.fbs",
+ ],
+ gen_reflections = 1,
)
genrule(
@@ -40,12 +64,13 @@
"-lm",
],
deps = [
- ":claw_queue",
+ ":claw_goal_fbs",
+ ":claw_output_fbs",
+ ":claw_position_fbs",
+ ":claw_status_fbs",
"//aos:math",
"//aos/controls:control_loop",
"//aos/controls:polytope",
- "//aos/logging:matrix_logging",
- "//aos/logging:queue_logging",
"//frc971/control_loops:coerce_goal",
"//frc971/control_loops:hall_effect_tracker",
"//frc971/control_loops:state_feedback_loop",
@@ -58,9 +83,13 @@
srcs = [
"claw_lib_test.cc",
],
+ data = ["//y2014:config.json"],
deps = [
+ ":claw_goal_fbs",
":claw_lib",
- ":claw_queue",
+ ":claw_output_fbs",
+ ":claw_position_fbs",
+ ":claw_status_fbs",
"//aos/controls:control_loop_test",
"//aos/testing:googletest",
"//frc971/control_loops:state_feedback_loop",
@@ -76,6 +105,6 @@
deps = [
":claw_lib",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
],
)
diff --git a/y2014/control_loops/claw/claw.cc b/y2014/control_loops/claw/claw.cc
index 14dbf11..56ca6b0 100644
--- a/y2014/control_loops/claw/claw.cc
+++ b/y2014/control_loops/claw/claw.cc
@@ -2,10 +2,7 @@
#include <algorithm>
-#include "aos/controls/control_loops.q.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
-#include "aos/logging/matrix_logging.h"
#include "aos/commonmath.h"
#include "y2014/constants.h"
@@ -46,11 +43,11 @@
namespace y2014 {
namespace control_loops {
+namespace claw {
using ::frc971::HallEffectTracker;
using ::y2014::control_loops::claw::kDt;
using ::frc971::control_loops::DoCoerceGoal;
-using ::y2014::control_loops::ClawPositionToLog;
static const double kZeroingVoltage = 4.0;
static const double kMaxVoltage = 12.0;
@@ -94,7 +91,7 @@
double max_voltage = is_zeroing_ ? kZeroingVoltage : kMaxVoltage;
if (::std::abs(u_bottom) > max_voltage || ::std::abs(u_top) > max_voltage) {
- AOS_LOG_MATRIX(DEBUG, "U at start", U());
+ VLOG(1) << "U at start " << U();
// H * U <= k
// U = UPos + UVel
// H * (UPos + UVel) <= k
@@ -116,7 +113,7 @@
position_error << error(0, 0), error(1, 0);
Eigen::Matrix<double, 2, 1> velocity_error;
velocity_error << error(2, 0), error(3, 0);
- AOS_LOG_MATRIX(DEBUG, "error", error);
+ VLOG(1) << "error " << error;
const auto &poly = is_zeroing_ ? U_Poly_zeroing_ : U_Poly_;
const Eigen::Matrix<double, 4, 2> pos_poly_H = poly.H() * position_K;
@@ -183,9 +180,9 @@
}
}
- AOS_LOG_MATRIX(DEBUG, "adjusted_pos_error", adjusted_pos_error);
+ VLOG(1) << "adjusted_pos_error " << adjusted_pos_error;
mutable_U() = velocity_K * velocity_error + position_K * adjusted_pos_error;
- AOS_LOG_MATRIX(DEBUG, "U is now", U());
+ VLOG(1) << "U is now" << U();
{
const auto values = constants::GetValues().claw;
@@ -221,69 +218,68 @@
encoder_(0.0),
last_encoder_(0.0) {}
-void ZeroedStateFeedbackLoop::SetPositionValues(
- const ::y2014::control_loops::HalfClawPosition &claw) {
- front_.Update(claw.front);
- calibration_.Update(claw.calibration);
- back_.Update(claw.back);
+void ZeroedStateFeedbackLoop::SetPositionValues(const HalfClawPosition *claw) {
+ front_.Update(claw->front());
+ calibration_.Update(claw->calibration());
+ back_.Update(claw->back());
bool any_sensor_triggered = any_triggered();
if (any_sensor_triggered && any_triggered_last_) {
// We are still on the hall effect and nothing has changed.
min_hall_effect_on_angle_ =
- ::std::min(min_hall_effect_on_angle_, claw.position);
+ ::std::min(min_hall_effect_on_angle_, claw->position());
max_hall_effect_on_angle_ =
- ::std::max(max_hall_effect_on_angle_, claw.position);
+ ::std::max(max_hall_effect_on_angle_, claw->position());
} else if (!any_sensor_triggered && !any_triggered_last_) {
// We are still off the hall effect and nothing has changed.
min_hall_effect_off_angle_ =
- ::std::min(min_hall_effect_off_angle_, claw.position);
+ ::std::min(min_hall_effect_off_angle_, claw->position());
max_hall_effect_off_angle_ =
- ::std::max(max_hall_effect_off_angle_, claw.position);
+ ::std::max(max_hall_effect_off_angle_, claw->position());
}
if (front_.is_posedge()) {
// Saw a posedge on the hall effect. Reset the limits.
min_hall_effect_on_angle_ =
- ::std::min(claw.front.posedge_value, claw.position);
+ ::std::min(claw->front()->posedge_value(), claw->position());
max_hall_effect_on_angle_ =
- ::std::max(claw.front.posedge_value, claw.position);
+ ::std::max(claw->front()->posedge_value(), claw->position());
}
if (calibration_.is_posedge()) {
// Saw a posedge on the hall effect. Reset the limits.
min_hall_effect_on_angle_ =
- ::std::min(claw.calibration.posedge_value, claw.position);
+ ::std::min(claw->calibration()->posedge_value(), claw->position());
max_hall_effect_on_angle_ =
- ::std::max(claw.calibration.posedge_value, claw.position);
+ ::std::max(claw->calibration()->posedge_value(), claw->position());
}
if (back_.is_posedge()) {
// Saw a posedge on the hall effect. Reset the limits.
min_hall_effect_on_angle_ =
- ::std::min(claw.back.posedge_value, claw.position);
+ ::std::min(claw->back()->posedge_value(), claw->position());
max_hall_effect_on_angle_ =
- ::std::max(claw.back.posedge_value, claw.position);
+ ::std::max(claw->back()->posedge_value(), claw->position());
}
if (front_.is_negedge()) {
// Saw a negedge on the hall effect. Reset the limits.
min_hall_effect_off_angle_ =
- ::std::min(claw.front.negedge_value, claw.position);
+ ::std::min(claw->front()->negedge_value(), claw->position());
max_hall_effect_off_angle_ =
- ::std::max(claw.front.negedge_value, claw.position);
+ ::std::max(claw->front()->negedge_value(), claw->position());
}
if (calibration_.is_negedge()) {
// Saw a negedge on the hall effect. Reset the limits.
min_hall_effect_off_angle_ =
- ::std::min(claw.calibration.negedge_value, claw.position);
+ ::std::min(claw->calibration()->negedge_value(), claw->position());
max_hall_effect_off_angle_ =
- ::std::max(claw.calibration.negedge_value, claw.position);
+ ::std::max(claw->calibration()->negedge_value(), claw->position());
}
if (back_.is_negedge()) {
// Saw a negedge on the hall effect. Reset the limits.
min_hall_effect_off_angle_ =
- ::std::min(claw.back.negedge_value, claw.position);
+ ::std::min(claw->back()->negedge_value(), claw->position());
max_hall_effect_off_angle_ =
- ::std::max(claw.back.negedge_value, claw.position);
+ ::std::max(claw->back()->negedge_value(), claw->position());
}
last_encoder_ = encoder_;
@@ -292,23 +288,22 @@
} else {
last_off_encoder_ = encoder_;
}
- encoder_ = claw.position;
+ encoder_ = claw->position();
any_triggered_last_ = any_sensor_triggered;
}
-void ZeroedStateFeedbackLoop::Reset(
- const ::y2014::control_loops::HalfClawPosition &claw) {
+void ZeroedStateFeedbackLoop::Reset(const HalfClawPosition *claw) {
set_zeroing_state(ZeroedStateFeedbackLoop::UNKNOWN_POSITION);
- front_.Reset(claw.front);
- calibration_.Reset(claw.calibration);
- back_.Reset(claw.back);
+ front_.Reset(claw->front());
+ calibration_.Reset(claw->calibration());
+ back_.Reset(claw->back());
// close up the min and max edge positions as they are no longer valid and
// will be expanded in future iterations
- min_hall_effect_on_angle_ = claw.position;
- max_hall_effect_on_angle_ = claw.position;
- min_hall_effect_off_angle_ = claw.position;
- max_hall_effect_off_angle_ = claw.position;
+ min_hall_effect_on_angle_ = claw->position();
+ max_hall_effect_on_angle_ = claw->position();
+ min_hall_effect_off_angle_ = claw->position();
+ max_hall_effect_off_angle_ = claw->position();
any_triggered_last_ = any_triggered();
}
@@ -374,8 +369,8 @@
}
ClawMotor::ClawMotor(::aos::EventLoop *event_loop, const ::std::string &name)
- : aos::controls::ControlLoop<::y2014::control_loops::ClawQueue>(event_loop,
- name),
+ : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name),
has_top_claw_goal_(false),
top_claw_goal_(0.0),
top_claw_(this),
@@ -563,45 +558,33 @@
// first update position based on angle limit
const double separation = *top_goal - *bottom_goal;
if (separation > values.claw.soft_max_separation) {
- AOS_LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
const double dsep = (separation - values.claw.soft_max_separation) / 2.0;
*bottom_goal += dsep;
*top_goal -= dsep;
- AOS_LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
}
if (separation < values.claw.soft_min_separation) {
- AOS_LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
const double dsep = (separation - values.claw.soft_min_separation) / 2.0;
*bottom_goal += dsep;
*top_goal -= dsep;
- AOS_LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
}
// now move both goals in unison
if (*bottom_goal < values.claw.lower_claw.lower_limit) {
- AOS_LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
*top_goal += values.claw.lower_claw.lower_limit - *bottom_goal;
*bottom_goal = values.claw.lower_claw.lower_limit;
- AOS_LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
}
if (*bottom_goal > values.claw.lower_claw.upper_limit) {
- AOS_LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
*top_goal -= *bottom_goal - values.claw.lower_claw.upper_limit;
*bottom_goal = values.claw.lower_claw.upper_limit;
- AOS_LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
}
if (*top_goal < values.claw.upper_claw.lower_limit) {
- AOS_LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
*bottom_goal += values.claw.upper_claw.lower_limit - *top_goal;
*top_goal = values.claw.upper_claw.lower_limit;
- AOS_LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
}
if (*top_goal > values.claw.upper_claw.upper_limit) {
- AOS_LOG_STRUCT(DEBUG, "before", ClawPositionToLog(*top_goal, *bottom_goal));
*bottom_goal -= *top_goal - values.claw.upper_claw.upper_limit;
*top_goal = values.claw.upper_claw.upper_limit;
- AOS_LOG_STRUCT(DEBUG, "after", ClawPositionToLog(*top_goal, *bottom_goal));
}
}
@@ -609,7 +592,7 @@
return (
(top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED &&
bottom_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED) ||
- ((has_joystick_state() ? joystick_state().autonomous : true) &&
+ ((has_joystick_state() ? joystick_state().autonomous() : true) &&
((top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
top_claw_.zeroing_state() ==
ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
@@ -621,43 +604,45 @@
bool ClawMotor::is_zeroing() const { return !is_ready(); }
// Positive angle is up, and positive power is up.
-void ClawMotor::RunIteration(
- const ::y2014::control_loops::ClawQueue::Goal *goal,
- const ::y2014::control_loops::ClawQueue::Position *position,
- ::y2014::control_loops::ClawQueue::Output *output,
- ::y2014::control_loops::ClawQueue::Status *status) {
+void ClawMotor::RunIteration(const Goal *goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) {
// Disable the motors now so that all early returns will return with the
// motors disabled.
+ OutputT output_struct;
if (output) {
- output->top_claw_voltage = 0;
- output->bottom_claw_voltage = 0;
- output->intake_voltage = 0;
- output->tusk_voltage = 0;
+ output_struct.top_claw_voltage = 0;
+ output_struct.bottom_claw_voltage = 0;
+ output_struct.intake_voltage = 0;
+ output_struct.tusk_voltage = 0;
}
+ StatusT status_struct;
if (goal) {
- if (::std::isnan(goal->bottom_angle) ||
- ::std::isnan(goal->separation_angle) || ::std::isnan(goal->intake) ||
- ::std::isnan(goal->centering)) {
+ if (::std::isnan(goal->bottom_angle()) ||
+ ::std::isnan(goal->separation_angle()) ||
+ ::std::isnan(goal->intake()) || ::std::isnan(goal->centering())) {
+ status->Send(Status::Pack(*status->fbb(), &status_struct));
+ output->Send(Output::Pack(*output->fbb(), &output_struct));
return;
}
}
if (WasReset()) {
- top_claw_.Reset(position->top);
- bottom_claw_.Reset(position->bottom);
+ top_claw_.Reset(position->top());
+ bottom_claw_.Reset(position->bottom());
}
const constants::Values &values = constants::GetValues();
if (position) {
Eigen::Matrix<double, 2, 1> Y;
- Y << position->bottom.position + bottom_claw_.offset(),
- position->top.position + top_claw_.offset();
+ Y << position->bottom()->position() + bottom_claw_.offset(),
+ position->top()->position() + top_claw_.offset();
claw_.Correct(Y);
- top_claw_.SetPositionValues(position->top);
- bottom_claw_.SetPositionValues(position->bottom);
+ top_claw_.SetPositionValues(position->top());
+ bottom_claw_.SetPositionValues(position->bottom());
if (!has_top_claw_goal_) {
has_top_claw_goal_ = true;
@@ -671,15 +656,12 @@
initial_separation_ =
top_claw_.absolute_position() - bottom_claw_.absolute_position();
}
- AOS_LOG_STRUCT(DEBUG, "absolute position",
- ClawPositionToLog(top_claw_.absolute_position(),
- bottom_claw_.absolute_position()));
}
bool autonomous, enabled;
if (has_joystick_state()) {
- autonomous = joystick_state().autonomous;
- enabled = joystick_state().enabled;
+ autonomous = joystick_state().autonomous();
+ enabled = joystick_state().enabled();
} else {
autonomous = true;
enabled = false;
@@ -700,8 +682,8 @@
ZeroedStateFeedbackLoop::DISABLED_CALIBRATION))))) {
// Ready to use the claw.
// Limit the goals here.
- bottom_claw_goal_ = goal->bottom_angle;
- top_claw_goal_ = goal->bottom_angle + goal->separation_angle;
+ bottom_claw_goal_ = goal->bottom_angle();
+ top_claw_goal_ = goal->bottom_angle() + goal->separation_angle();
has_bottom_claw_goal_ = true;
has_top_claw_goal_ = true;
doing_calibration_fine_tune_ = false;
@@ -759,7 +741,7 @@
bottom_claw_.back())) {
// do calibration
bottom_claw_.SetCalibration(
- position->bottom.calibration.posedge_value,
+ position->bottom()->calibration()->posedge_value(),
values.claw.lower_claw.calibration.lower_angle);
bottom_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
// calibrated so we are done fine tuning bottom
@@ -816,7 +798,7 @@
top_claw_.front(), top_claw_.back())) {
// do calibration
top_claw_.SetCalibration(
- position->top.calibration.posedge_value,
+ position->top()->calibration()->posedge_value(),
values.claw.upper_claw.calibration.lower_angle);
top_claw_.set_zeroing_state(ZeroedStateFeedbackLoop::CALIBRATED);
// calibrated so we are done fine tuning top
@@ -836,10 +818,11 @@
doing_calibration_fine_tune_ = false;
if (!was_enabled_ && enabled) {
if (position) {
- top_claw_goal_ = position->top.position + top_claw_.offset();
- bottom_claw_goal_ = position->bottom.position + bottom_claw_.offset();
+ top_claw_goal_ = position->top()->position() + top_claw_.offset();
+ bottom_claw_goal_ =
+ position->bottom()->position() + bottom_claw_.offset();
initial_separation_ =
- position->top.position - position->bottom.position;
+ position->top()->position() - position->bottom()->position();
} else {
has_top_claw_goal_ = false;
has_bottom_claw_goal_ = false;
@@ -907,7 +890,6 @@
if (has_top_claw_goal_ && has_bottom_claw_goal_) {
claw_.mutable_R() << bottom_claw_goal_, top_claw_goal_ - bottom_claw_goal_,
bottom_claw_velocity_, top_claw_velocity_ - bottom_claw_velocity_;
- AOS_LOG_MATRIX(DEBUG, "actual goal", claw_.R());
// Only cap power when one of the halves of the claw is moving slowly and
// could wind up.
@@ -973,55 +955,60 @@
if (output) {
if (goal) {
//setup the intake
- output->intake_voltage =
- (goal->intake > 12.0) ? 12 : (goal->intake < -12.0) ? -12.0
- : goal->intake;
- output->tusk_voltage = goal->centering;
- output->tusk_voltage =
- (goal->centering > 12.0) ? 12 : (goal->centering < -12.0)
+ output_struct.intake_voltage =
+ (goal->intake() > 12.0)
+ ? 12
+ : (goal->intake() < -12.0) ? -12.0 : goal->intake();
+ output_struct.tusk_voltage = goal->centering();
+ output_struct.tusk_voltage =
+ (goal->centering() > 12.0) ? 12 : (goal->centering() < -12.0)
? -12.0
- : goal->centering;
+ : goal->centering();
}
- output->top_claw_voltage = claw_.U(1, 0);
- output->bottom_claw_voltage = claw_.U(0, 0);
+ output_struct.top_claw_voltage = claw_.U(1, 0);
+ output_struct.bottom_claw_voltage = claw_.U(0, 0);
- if (output->top_claw_voltage > kMaxVoltage) {
- output->top_claw_voltage = kMaxVoltage;
- } else if (output->top_claw_voltage < -kMaxVoltage) {
- output->top_claw_voltage = -kMaxVoltage;
+ if (output_struct.top_claw_voltage > kMaxVoltage) {
+ output_struct.top_claw_voltage = kMaxVoltage;
+ } else if (output_struct.top_claw_voltage < -kMaxVoltage) {
+ output_struct.top_claw_voltage = -kMaxVoltage;
}
- if (output->bottom_claw_voltage > kMaxVoltage) {
- output->bottom_claw_voltage = kMaxVoltage;
- } else if (output->bottom_claw_voltage < -kMaxVoltage) {
- output->bottom_claw_voltage = -kMaxVoltage;
+ if (output_struct.bottom_claw_voltage > kMaxVoltage) {
+ output_struct.bottom_claw_voltage = kMaxVoltage;
+ } else if (output_struct.bottom_claw_voltage < -kMaxVoltage) {
+ output_struct.bottom_claw_voltage = -kMaxVoltage;
}
+
+ output->Send(Output::Pack(*output->fbb(), &output_struct));
}
- status->bottom = bottom_absolute_position();
- status->separation = top_absolute_position() - bottom_absolute_position();
- status->bottom_velocity = claw_.X_hat(2, 0);
- status->separation_velocity = claw_.X_hat(3, 0);
+ status_struct.bottom = bottom_absolute_position();
+ status_struct.separation =
+ top_absolute_position() - bottom_absolute_position();
+ status_struct.bottom_velocity = claw_.X_hat(2, 0);
+ status_struct.separation_velocity = claw_.X_hat(3, 0);
if (goal) {
bool bottom_done =
- ::std::abs(bottom_absolute_position() - goal->bottom_angle) < 0.020;
- bool bottom_velocity_done = ::std::abs(status->bottom_velocity) < 0.2;
+ ::std::abs(bottom_absolute_position() - goal->bottom_angle()) < 0.020;
+ bool bottom_velocity_done = ::std::abs(status_struct.bottom_velocity) < 0.2;
bool separation_done =
::std::abs((top_absolute_position() - bottom_absolute_position()) -
- goal->separation_angle) < 0.020;
+ goal->separation_angle()) < 0.020;
bool separation_done_with_ball =
::std::abs((top_absolute_position() - bottom_absolute_position()) -
- goal->separation_angle) < 0.06;
- status->done = is_ready() && separation_done && bottom_done && bottom_velocity_done;
- status->done_with_ball =
- is_ready() && separation_done_with_ball && bottom_done && bottom_velocity_done;
+ goal->separation_angle()) < 0.06;
+ status_struct.done =
+ is_ready() && separation_done && bottom_done && bottom_velocity_done;
+ status_struct.done_with_ball = is_ready() && separation_done_with_ball &&
+ bottom_done && bottom_velocity_done;
} else {
- status->done = status->done_with_ball = false;
+ status_struct.done = status_struct.done_with_ball = false;
}
- status->zeroed = is_ready();
- status->zeroed_for_auto =
+ status_struct.zeroed = is_ready();
+ status_struct.zeroed_for_auto =
(top_claw_.zeroing_state() == ZeroedStateFeedbackLoop::CALIBRATED ||
top_claw_.zeroing_state() ==
ZeroedStateFeedbackLoop::DISABLED_CALIBRATION) &&
@@ -1029,8 +1016,11 @@
bottom_claw_.zeroing_state() ==
ZeroedStateFeedbackLoop::DISABLED_CALIBRATION);
+ status->Send(Status::Pack(*status->fbb(), &status_struct));
+
was_enabled_ = enabled;
}
+} // namespace claw
} // namespace control_loops
} // namespace y2014
diff --git a/y2014/control_loops/claw/claw.h b/y2014/control_loops/claw/claw.h
index 082ddce..5bcccc1 100644
--- a/y2014/control_loops/claw/claw.h
+++ b/y2014/control_loops/claw/claw.h
@@ -5,15 +5,19 @@
#include "aos/controls/control_loop.h"
#include "aos/controls/polytope.h"
-#include "y2014/constants.h"
-#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/coerce_goal.h"
-#include "y2014/control_loops/claw/claw.q.h"
-#include "y2014/control_loops/claw/claw_motor_plant.h"
#include "frc971/control_loops/hall_effect_tracker.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "y2014/constants.h"
+#include "y2014/control_loops/claw/claw_goal_generated.h"
+#include "y2014/control_loops/claw/claw_motor_plant.h"
+#include "y2014/control_loops/claw/claw_output_generated.h"
+#include "y2014/control_loops/claw/claw_position_generated.h"
+#include "y2014/control_loops/claw/claw_status_generated.h"
namespace y2014 {
namespace control_loops {
+namespace claw {
namespace testing {
class WindupClawTest;
};
@@ -77,9 +81,9 @@
}
JointZeroingState zeroing_state() const { return zeroing_state_; }
- void SetPositionValues(const ::y2014::control_loops::HalfClawPosition &claw);
+ void SetPositionValues(const HalfClawPosition *claw);
- void Reset(const ::y2014::control_loops::HalfClawPosition &claw);
+ void Reset(const HalfClawPosition *claw);
double absolute_position() const { return encoder() + offset(); }
@@ -183,11 +187,10 @@
};
class ClawMotor
- : public aos::controls::ControlLoop<::y2014::control_loops::ClawQueue> {
+ : public aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
- explicit ClawMotor(
- ::aos::EventLoop *event_loop,
- const ::std::string &name = ".y2014.control_loops.claw_queue");
+ explicit ClawMotor(::aos::EventLoop *event_loop,
+ const ::std::string &name = "/claw");
// True if the state machine is ready.
bool capped_goal() const { return capped_goal_; }
@@ -217,11 +220,9 @@
CalibrationMode mode() const { return mode_; }
protected:
- virtual void RunIteration(
- const ::y2014::control_loops::ClawQueue::Goal *goal,
- const ::y2014::control_loops::ClawQueue::Position *position,
- ::y2014::control_loops::ClawQueue::Output *output,
- ::y2014::control_loops::ClawQueue::Status *status);
+ virtual void RunIteration(const Goal *goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status);
double top_absolute_position() const {
return claw_.X_hat(1, 0) + claw_.X_hat(0, 0);
@@ -262,6 +263,7 @@
void LimitClawGoal(double *bottom_goal, double *top_goal,
const constants::Values &values);
+} // namespace claw
} // namespace control_loops
} // namespace y2014
diff --git a/y2014/control_loops/claw/claw.q b/y2014/control_loops/claw/claw.q
deleted file mode 100644
index ff4c8b8..0000000
--- a/y2014/control_loops/claw/claw.q
+++ /dev/null
@@ -1,74 +0,0 @@
-package y2014.control_loops;
-
-import "aos/controls/control_loops.q";
-import "frc971/control_loops/control_loops.q";
-
-struct HalfClawPosition {
- // The current position of this half of the claw.
- double position;
-
- // The hall effect sensor at the front limit.
- .frc971.HallEffectStruct front;
- // The hall effect sensor in the middle to use for real calibration.
- .frc971.HallEffectStruct calibration;
- // The hall effect at the back limit.
- .frc971.HallEffectStruct back;
-};
-
-// All angles here are 0 vertical, positive "up" (aka backwards).
-// Published on ".y2014.control_loops.claw_queue"
-queue_group ClawQueue {
- implements aos.control_loops.ControlLoop;
-
- message Goal {
- // The angle of the bottom claw.
- double bottom_angle;
- // How much higher the top claw is.
- double separation_angle;
- // top claw intake roller
- double intake;
- // bottom claw tusk centering
- double centering;
- };
-
- message Position {
- // All the top claw information.
- HalfClawPosition top;
- // All the bottom claw information.
- HalfClawPosition bottom;
- };
-
- message Output {
- double intake_voltage;
- double top_claw_voltage;
- double bottom_claw_voltage;
- double tusk_voltage;
- };
-
- message Status {
- // True if zeroed enough for the current period (autonomous or teleop).
- bool zeroed;
- // True if zeroed as much as we will force during autonomous.
- bool zeroed_for_auto;
- // True if zeroed and within tolerance for separation and bottom angle.
- bool done;
- // True if zeroed and within tolerance for separation and bottom angle.
- // seperation allowance much wider as a ball may be included
- bool done_with_ball;
- // Dump the values of the state matrix.
- double bottom;
- double bottom_velocity;
- double separation;
- double separation_velocity;
- };
-
- queue Goal goal;
- queue Position position;
- queue Output output;
- queue Status status;
-};
-
-struct ClawPositionToLog {
- double top;
- double bottom;
-};
diff --git a/y2014/control_loops/claw/claw_goal.fbs b/y2014/control_loops/claw/claw_goal.fbs
new file mode 100644
index 0000000..eb5860c
--- /dev/null
+++ b/y2014/control_loops/claw/claw_goal.fbs
@@ -0,0 +1,15 @@
+namespace y2014.control_loops.claw;
+
+// All angles here are 0 vertical, positive "up" (aka backwards).
+table Goal {
+ // The angle of the bottom claw.
+ bottom_angle:double;
+ // How much higher the top claw is.
+ separation_angle:double;
+ // top claw intake roller
+ intake:double;
+ // bottom claw tusk centering
+ centering:double;
+}
+
+root_type Goal;
diff --git a/y2014/control_loops/claw/claw_lib_test.cc b/y2014/control_loops/claw/claw_lib_test.cc
index 41dbfca..55e186e 100644
--- a/y2014/control_loops/claw/claw_lib_test.cc
+++ b/y2014/control_loops/claw/claw_lib_test.cc
@@ -8,16 +8,18 @@
#include "gtest/gtest.h"
#include "y2014/constants.h"
#include "y2014/control_loops/claw/claw.h"
-#include "y2014/control_loops/claw/claw.q.h"
+#include "y2014/control_loops/claw/claw_goal_generated.h"
#include "y2014/control_loops/claw/claw_motor_plant.h"
+#include "y2014/control_loops/claw/claw_output_generated.h"
+#include "y2014/control_loops/claw/claw_position_generated.h"
+#include "y2014/control_loops/claw/claw_status_generated.h"
namespace y2014 {
namespace control_loops {
+namespace claw {
namespace testing {
-using ::y2014::control_loops::claw::MakeClawPlant;
-using ::frc971::HallEffectStruct;
-using ::y2014::control_loops::HalfClawPosition;
+using ::frc971::HallEffectStructT;
using ::aos::monotonic_clock;
namespace chrono = ::std::chrono;
@@ -38,10 +40,8 @@
double initial_top_position,
double initial_bottom_position)
: event_loop_(event_loop),
- claw_position_sender_(event_loop_->MakeSender<ClawQueue::Position>(
- ".y2014.control_loops.claw_queue.position")),
- claw_output_fetcher_(event_loop_->MakeFetcher<ClawQueue::Output>(
- ".y2014.control_loops.claw_queue.output")),
+ claw_position_sender_(event_loop_->MakeSender<Position>("/claw")),
+ claw_output_fetcher_(event_loop_->MakeFetcher<Output>("/claw")),
claw_plant_(new StateFeedbackPlant<4, 2, 2>(MakeClawPlant())) {
Reinitialize(initial_top_position, initial_bottom_position);
@@ -69,7 +69,9 @@
ReinitializePartial(TOP_CLAW, initial_top_position);
ReinitializePartial(BOTTOM_CLAW, initial_bottom_position);
- last_position_.Zero();
+
+ last_position_.top.reset();
+ last_position_.bottom.reset();
SetPhysicalSensors(&last_position_);
}
@@ -99,44 +101,58 @@
void SetHallEffect(double pos,
const constants::Values::Claws::AnglePair &pair,
- HallEffectStruct *hall_effect) {
+ HallEffectStructT *hall_effect) {
hall_effect->current = CheckRange(pos, pair);
}
void SetClawHallEffects(double pos,
const constants::Values::Claws::Claw &claw,
- HalfClawPosition *half_claw) {
- SetHallEffect(pos, claw.front, &half_claw->front);
- SetHallEffect(pos, claw.calibration, &half_claw->calibration);
- SetHallEffect(pos, claw.back, &half_claw->back);
+ HalfClawPositionT *half_claw) {
+ if (!half_claw->front) {
+ half_claw->front.reset(new HallEffectStructT());
+ }
+ if (!half_claw->calibration) {
+ half_claw->calibration.reset(new HallEffectStructT());
+ }
+ if (!half_claw->back) {
+ half_claw->back.reset(new HallEffectStructT());
+ }
+ SetHallEffect(pos, claw.front, half_claw->front.get());
+ SetHallEffect(pos, claw.calibration, half_claw->calibration.get());
+ SetHallEffect(pos, claw.back, half_claw->back.get());
}
// Sets the values of the physical sensors that can be directly observed
// (encoder, hall effect).
- void SetPhysicalSensors(
- ::y2014::control_loops::ClawQueue::Position *position) {
- position->top.position = GetPosition(TOP_CLAW);
- position->bottom.position = GetPosition(BOTTOM_CLAW);
+ void SetPhysicalSensors(PositionT *position) {
+ if (!position->top) {
+ position->top.reset(new HalfClawPositionT());
+ }
+ if (!position->bottom) {
+ position->bottom.reset(new HalfClawPositionT());
+ }
+
+ position->top->position = GetPosition(TOP_CLAW);
+ position->bottom->position = GetPosition(BOTTOM_CLAW);
double pos[2] = {GetAbsolutePosition(TOP_CLAW),
GetAbsolutePosition(BOTTOM_CLAW)};
AOS_LOG(DEBUG, "Physical claws are at {top: %f, bottom: %f}\n",
pos[TOP_CLAW], pos[BOTTOM_CLAW]);
- const constants::Values& values = constants::GetValues();
+ const constants::Values &values = constants::GetValues();
// Signal that each hall effect sensor has been triggered if it is within
// the correct range.
- SetClawHallEffects(pos[TOP_CLAW], values.claw.upper_claw, &position->top);
+ SetClawHallEffects(pos[TOP_CLAW], values.claw.upper_claw,
+ position->top.get());
SetClawHallEffects(pos[BOTTOM_CLAW], values.claw.lower_claw,
- &position->bottom);
+ position->bottom.get());
}
- void UpdateHallEffect(double angle,
- double last_angle,
- double initial_position,
- HallEffectStruct *position,
- const HallEffectStruct &last_position,
+ void UpdateHallEffect(double angle, double last_angle,
+ double initial_position, HallEffectStructT *position,
+ const HallEffectStructT &last_position,
const constants::Values::Claws::AnglePair &pair,
const char *claw_name, const char *hall_effect_name) {
if (position->current && !last_position.current) {
@@ -167,49 +183,64 @@
}
}
- void UpdateClawHallEffects(
- HalfClawPosition *position,
- const HalfClawPosition &last_position,
- const constants::Values::Claws::Claw &claw, double initial_position,
- const char *claw_name) {
- UpdateHallEffect(position->position + initial_position,
+ void UpdateClawHallEffects(HalfClawPositionT *half_claw,
+ const HalfClawPositionT &last_position,
+ const constants::Values::Claws::Claw &claw,
+ double initial_position, const char *claw_name) {
+ if (!half_claw->front) {
+ half_claw->front.reset(new HallEffectStructT());
+ }
+ if (!half_claw->calibration) {
+ half_claw->calibration.reset(new HallEffectStructT());
+ }
+ if (!half_claw->back) {
+ half_claw->back.reset(new HallEffectStructT());
+ }
+ UpdateHallEffect(half_claw->position + initial_position,
last_position.position + initial_position,
- initial_position, &position->front, last_position.front,
- claw.front, claw_name, "front");
- UpdateHallEffect(position->position + initial_position,
+ initial_position, half_claw->front.get(),
+ *last_position.front.get(), claw.front, claw_name, "front");
+ UpdateHallEffect(half_claw->position + initial_position,
last_position.position + initial_position,
- initial_position, &position->calibration,
- last_position.calibration, claw.calibration, claw_name,
- "calibration");
- UpdateHallEffect(position->position + initial_position,
+ initial_position, half_claw->calibration.get(),
+ *last_position.calibration.get(), claw.calibration,
+ claw_name, "calibration");
+ UpdateHallEffect(half_claw->position + initial_position,
last_position.position + initial_position,
- initial_position, &position->back, last_position.back,
- claw.back, claw_name, "back");
+ initial_position, half_claw->back.get(),
+ *last_position.back.get(), claw.back, claw_name, "back");
}
// Sends out the position queue messages.
void SendPositionMessage() {
- ::aos::Sender<::y2014::control_loops::ClawQueue::Position>::Message
- position = claw_position_sender_.MakeMessage();
+ ::aos::Sender<Position>::Builder builder =
+ claw_position_sender_.MakeBuilder();
// Initialize all the counters to their previous values.
- *position = last_position_;
- SetPhysicalSensors(position.get());
+ PositionT position;
- const constants::Values& values = constants::GetValues();
+ flatbuffers::FlatBufferBuilder fbb;
+ flatbuffers::Offset<Position> position_offset =
+ Position::Pack(fbb, &last_position_);
+ fbb.Finish(position_offset);
- UpdateClawHallEffects(&position->top, last_position_.top,
+ flatbuffers::GetRoot<Position>(fbb.GetBufferPointer())->UnPackTo(&position);
+ SetPhysicalSensors(&position);
+
+ const constants::Values &values = constants::GetValues();
+
+ UpdateClawHallEffects(position.top.get(), *last_position_.top.get(),
values.claw.upper_claw, initial_position_[TOP_CLAW],
"Top");
- UpdateClawHallEffects(&position->bottom, last_position_.bottom,
+ UpdateClawHallEffects(position.bottom.get(), *last_position_.bottom.get(),
values.claw.lower_claw,
initial_position_[BOTTOM_CLAW], "Bottom");
// Only set calibration if it changed last cycle. Calibration starts out
// with a value of 0.
- last_position_ = *position;
- position.Send();
+ builder.Send(Position::Pack(*builder.fbb(), &position));
+ last_position_ = std::move(position);
}
// Simulates the claw moving for one timestep.
@@ -218,8 +249,8 @@
EXPECT_TRUE(claw_output_fetcher_.Fetch());
Eigen::Matrix<double, 2, 1> U;
- U << claw_output_fetcher_->bottom_claw_voltage,
- claw_output_fetcher_->top_claw_voltage;
+ U << claw_output_fetcher_->bottom_claw_voltage(),
+ claw_output_fetcher_->top_claw_voltage();
claw_plant_->Update(U);
// Check that the claw is within the limits.
@@ -237,8 +268,8 @@
private:
::aos::EventLoop *event_loop_;
- ::aos::Sender<ClawQueue::Position> claw_position_sender_;
- ::aos::Fetcher<ClawQueue::Output> claw_output_fetcher_;
+ ::aos::Sender<Position> claw_position_sender_;
+ ::aos::Fetcher<Output> claw_output_fetcher_;
// The whole claw.
::std::unique_ptr<StateFeedbackPlant<4, 2, 2>> claw_plant_;
@@ -252,18 +283,18 @@
double initial_position_[CLAW_COUNT];
- ::y2014::control_loops::ClawQueue::Position last_position_;
+ PositionT last_position_;
};
class ClawTest : public ::aos::testing::ControlLoopTest {
protected:
ClawTest()
- : ::aos::testing::ControlLoopTest(chrono::microseconds(5000)),
+ : ::aos::testing::ControlLoopTest(
+ aos::configuration::ReadConfig("y2014/config.json"),
+ chrono::microseconds(5000)),
test_event_loop_(MakeEventLoop()),
- claw_goal_sender_(test_event_loop_->MakeSender<ClawQueue::Goal>(
- ".y2014.control_loops.claw_queue.goal")),
- claw_goal_fetcher_(test_event_loop_->MakeFetcher<ClawQueue::Goal>(
- ".y2014.control_loops.claw_queue.goal")),
+ claw_goal_sender_(test_event_loop_->MakeSender<Goal>("/claw")),
+ claw_goal_fetcher_(test_event_loop_->MakeFetcher<Goal>("/claw")),
claw_event_loop_(MakeEventLoop()),
claw_motor_(claw_event_loop_.get()),
claw_plant_event_loop_(MakeEventLoop()),
@@ -275,14 +306,14 @@
double bottom = claw_motor_plant_.GetAbsolutePosition(BOTTOM_CLAW);
double separation =
claw_motor_plant_.GetAbsolutePosition(TOP_CLAW) - bottom;
- EXPECT_NEAR(claw_goal_fetcher_->bottom_angle, bottom, 1e-4);
- EXPECT_NEAR(claw_goal_fetcher_->separation_angle, separation, 1e-4);
+ EXPECT_NEAR(claw_goal_fetcher_->bottom_angle(), bottom, 1e-4);
+ EXPECT_NEAR(claw_goal_fetcher_->separation_angle(), separation, 1e-4);
EXPECT_LE(min_separation_, separation);
}
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
- ::aos::Sender<ClawQueue::Goal> claw_goal_sender_;
- ::aos::Fetcher<ClawQueue::Goal> claw_goal_fetcher_;
+ ::aos::Sender<Goal> claw_goal_sender_;
+ ::aos::Fetcher<Goal> claw_goal_fetcher_;
// Create a loop and simulation plant.
::std::unique_ptr<::aos::EventLoop> claw_event_loop_;
@@ -294,15 +325,15 @@
// Minimum amount of acceptable separation between the top and bottom of the
// claw.
double min_separation_;
-
};
TEST_F(ClawTest, HandlesNAN) {
{
- auto message = claw_goal_sender_.MakeMessage();
- message->bottom_angle = ::std::nan("");
- message->separation_angle = ::std::nan("");
- EXPECT_TRUE(message.Send());
+ auto builder = claw_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_bottom_angle(::std::nan(""));
+ goal_builder.add_separation_angle(::std::nan(""));
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
SetEnabled(true);
@@ -312,10 +343,11 @@
// Tests that the wrist zeros correctly and goes to a position.
TEST_F(ClawTest, ZerosCorrectly) {
{
- auto message = claw_goal_sender_.MakeMessage();
- message->bottom_angle = 0.1;
- message->separation_angle = 0.2;
- EXPECT_TRUE(message.Send());
+ auto builder = claw_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_bottom_angle(0.1);
+ goal_builder.add_separation_angle(0.2);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
SetEnabled(true);
@@ -409,10 +441,11 @@
claw_motor_plant_.Reinitialize(GetParam().first, GetParam().second);
{
- auto message = claw_goal_sender_.MakeMessage();
- message->bottom_angle = 0.1;
- message->separation_angle = 0.2;
- EXPECT_TRUE(message.Send());
+ auto builder = claw_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_bottom_angle(0.1);
+ goal_builder.add_separation_angle(0.2);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
SetEnabled(true);
@@ -586,10 +619,11 @@
// zeroing position is saturating the goal.
TEST_F(WindupClawTest, NoWindupPositive) {
{
- auto message = claw_goal_sender_.MakeMessage();
- message->bottom_angle = 0.1;
- message->separation_angle = 0.2;
- EXPECT_TRUE(message.Send());
+ auto builder = claw_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_bottom_angle(0.1);
+ goal_builder.add_separation_angle(0.2);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
TestWindup(ClawMotor::UNKNOWN_LOCATION,
@@ -602,10 +636,11 @@
// zeroing position is saturating the goal.
TEST_F(WindupClawTest, NoWindupNegative) {
{
- auto message = claw_goal_sender_.MakeMessage();
- message->bottom_angle = 0.1;
- message->separation_angle = 0.2;
- EXPECT_TRUE(message.Send());
+ auto builder = claw_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_bottom_angle(0.1);
+ goal_builder.add_separation_angle(0.2);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
TestWindup(ClawMotor::UNKNOWN_LOCATION,
@@ -618,10 +653,11 @@
// zeroing position is saturating the goal.
TEST_F(WindupClawTest, NoWindupNegativeFineTune) {
{
- auto message = claw_goal_sender_.MakeMessage();
- message->bottom_angle = 0.1;
- message->separation_angle = 0.2;
- EXPECT_TRUE(message.Send());
+ auto builder = claw_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_bottom_angle(0.1);
+ goal_builder.add_separation_angle(0.2);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
TestWindup(ClawMotor::FINE_TUNE_BOTTOM,
@@ -631,5 +667,6 @@
}
} // namespace testing
+} // namespace claw
} // namespace control_loops
} // namespace y2014
diff --git a/y2014/control_loops/claw/claw_main.cc b/y2014/control_loops/claw/claw_main.cc
index 65627d5..f21c7c5 100644
--- a/y2014/control_loops/claw/claw_main.cc
+++ b/y2014/control_loops/claw/claw_main.cc
@@ -1,13 +1,16 @@
#include "y2014/control_loops/claw/claw.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
int main() {
::aos::InitNRT(true);
- ::aos::ShmEventLoop event_loop;
- ::y2014::control_loops::ClawMotor claw(&event_loop);
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
+ ::y2014::control_loops::claw::ClawMotor claw(&event_loop);
event_loop.Run();
diff --git a/y2014/control_loops/claw/claw_output.fbs b/y2014/control_loops/claw/claw_output.fbs
new file mode 100644
index 0000000..8cb42e0
--- /dev/null
+++ b/y2014/control_loops/claw/claw_output.fbs
@@ -0,0 +1,11 @@
+namespace y2014.control_loops.claw;
+
+// All angles here are 0 vertical, positive "up" (aka backwards).
+table Output {
+ intake_voltage:double;
+ top_claw_voltage:double;
+ bottom_claw_voltage:double;
+ tusk_voltage:double;
+}
+
+root_type Output;
diff --git a/y2014/control_loops/claw/claw_position.fbs b/y2014/control_loops/claw/claw_position.fbs
new file mode 100644
index 0000000..17d9f17
--- /dev/null
+++ b/y2014/control_loops/claw/claw_position.fbs
@@ -0,0 +1,25 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2014.control_loops.claw;
+
+// All angles here are 0 vertical, positive "up" (aka backwards).
+table HalfClawPosition {
+ // The current position of this half of the claw.
+ position:double;
+
+ // The hall effect sensor at the front limit.
+ front:frc971.HallEffectStruct;
+ // The hall effect sensor in the middle to use for real calibration.
+ calibration:frc971.HallEffectStruct;
+ // The hall effect at the back limit.
+ back:frc971.HallEffectStruct;
+}
+
+table Position {
+ // All the top claw information.
+ top:HalfClawPosition;
+ // All the bottom claw information.
+ bottom:HalfClawPosition;
+}
+
+root_type Position;
diff --git a/y2014/control_loops/claw/claw_status.fbs b/y2014/control_loops/claw/claw_status.fbs
new file mode 100644
index 0000000..eda9279
--- /dev/null
+++ b/y2014/control_loops/claw/claw_status.fbs
@@ -0,0 +1,21 @@
+namespace y2014.control_loops.claw;
+
+// All angles here are 0 vertical, positive "up" (aka backwards).
+table Status {
+ // True if zeroed enough for the current period (autonomous or teleop).
+ zeroed:bool;
+ // True if zeroed as much as we will force during autonomous.
+ zeroed_for_auto:bool;
+ // True if zeroed and within tolerance for separation and bottom angle.
+ done:bool;
+ // True if zeroed and within tolerance for separation and bottom angle.
+ // seperation allowance much wider as a ball may be included
+ done_with_ball:bool;
+ // Dump the values of the state matrix.
+ bottom:double;
+ bottom_velocity:double;
+ separation:double;
+ separation_velocity:double;
+}
+
+root_type Status;
diff --git a/y2014/control_loops/drivetrain/BUILD b/y2014/control_loops/drivetrain/BUILD
index 858f7e0..c458208 100644
--- a/y2014/control_loops/drivetrain/BUILD
+++ b/y2014/control_loops/drivetrain/BUILD
@@ -1,7 +1,5 @@
package(default_visibility = ["//visibility:public"])
-load("//aos/build:queues.bzl", "queue_library")
-
genrule(
name = "genrule_drivetrain",
outs = [
@@ -77,7 +75,7 @@
deps = [
":drivetrain_base",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
"//frc971/control_loops/drivetrain:drivetrain_lib",
],
)
diff --git a/y2014/control_loops/drivetrain/drivetrain_main.cc b/y2014/control_loops/drivetrain/drivetrain_main.cc
index 7749631..c210770 100644
--- a/y2014/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2014/control_loops/drivetrain/drivetrain_main.cc
@@ -1,6 +1,6 @@
#include "aos/init.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "y2014/control_loops/drivetrain/drivetrain_base.h"
@@ -9,7 +9,10 @@
int main() {
::aos::InitNRT(true);
- ::aos::ShmEventLoop event_loop;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
&event_loop, ::y2014::control_loops::GetDrivetrainConfig());
DrivetrainLoop drivetrain(::y2014::control_loops::GetDrivetrainConfig(),
diff --git a/y2014/control_loops/shooter/BUILD b/y2014/control_loops/shooter/BUILD
index 075cafc..a2073c9 100644
--- a/y2014/control_loops/shooter/BUILD
+++ b/y2014/control_loops/shooter/BUILD
@@ -1,16 +1,40 @@
package(default_visibility = ["//visibility:public"])
-load("//aos/build:queues.bzl", "queue_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
-queue_library(
- name = "shooter_queue",
+flatbuffer_cc_library(
+ name = "shooter_goal_fbs",
srcs = [
- "shooter.q",
+ "shooter_goal.fbs",
],
- deps = [
- "//aos/controls:control_loop_queues",
- "//frc971/control_loops:queues",
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "shooter_position_fbs",
+ srcs = [
+ "shooter_position.fbs",
],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "shooter_output_fbs",
+ srcs = [
+ "shooter_output.fbs",
+ ],
+ gen_reflections = 1,
+)
+
+flatbuffer_cc_library(
+ name = "shooter_status_fbs",
+ srcs = [
+ "shooter_status.fbs",
+ ],
+ gen_reflections = 1,
)
genrule(
@@ -44,9 +68,12 @@
"-lm",
],
deps = [
- ":shooter_queue",
+ ":shooter_goal_fbs",
+ ":shooter_output_fbs",
+ ":shooter_position_fbs",
+ ":shooter_status_fbs",
"//aos/controls:control_loop",
- "//aos/logging:queue_logging",
+ "//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:state_feedback_loop",
"//y2014:constants",
],
@@ -57,9 +84,13 @@
srcs = [
"shooter_lib_test.cc",
],
+ data = ["//y2014:config.json"],
deps = [
+ ":shooter_goal_fbs",
":shooter_lib",
- ":shooter_queue",
+ ":shooter_output_fbs",
+ ":shooter_position_fbs",
+ ":shooter_status_fbs",
"//aos/controls:control_loop_test",
"//aos/testing:googletest",
"//frc971/control_loops:state_feedback_loop",
@@ -75,6 +106,6 @@
deps = [
":shooter_lib",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
],
)
diff --git a/y2014/control_loops/shooter/shooter.cc b/y2014/control_loops/shooter/shooter.cc
index bf14aad..fb115c7 100644
--- a/y2014/control_loops/shooter/shooter.cc
+++ b/y2014/control_loops/shooter/shooter.cc
@@ -6,15 +6,13 @@
#include <limits>
#include <chrono>
-#include "aos/controls/control_loops.q.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
-
#include "y2014/constants.h"
#include "y2014/control_loops/shooter/shooter_motor_plant.h"
namespace y2014 {
namespace control_loops {
+namespace shooter {
namespace chrono = ::std::chrono;
using ::aos::monotonic_clock;
@@ -45,10 +43,6 @@
voltage_ = std::max(-max_voltage_, voltage_);
mutable_U(0, 0) = voltage_ - old_voltage;
- AOS_LOG_STRUCT(
- DEBUG, "shooter_output",
- ::y2014::control_loops::ShooterVoltageToLog(X_hat(2, 0), voltage_));
-
last_voltage_ = voltage_;
capped_goal_ = false;
}
@@ -67,8 +61,6 @@
mutable_R(0, 0) -= dx;
}
capped_goal_ = true;
- AOS_LOG_STRUCT(DEBUG, "to prevent windup",
- ::y2014::control_loops::ShooterMovingGoal(dx));
} else if (uncapped_voltage() < -max_voltage_) {
double dx;
if (index() == 0) {
@@ -82,8 +74,6 @@
mutable_R(0, 0) -= dx;
}
capped_goal_ = true;
- AOS_LOG_STRUCT(DEBUG, "to prevent windup",
- ::y2014::control_loops::ShooterMovingGoal(dx));
} else {
capped_goal_ = false;
}
@@ -100,7 +90,6 @@
void ZeroedStateFeedbackLoop::SetCalibration(double encoder_val,
double known_position) {
- double old_position = absolute_position();
double previous_offset = offset_;
offset_ = known_position - encoder_val;
double doffset = offset_ - previous_offset;
@@ -110,16 +99,12 @@
if (index() == 0) {
mutable_R(2, 0) += -plant().A(1, 0) / plant().A(1, 2) * (doffset);
}
- AOS_LOG_STRUCT(DEBUG, "sensor edge (fake?)",
- ::y2014::control_loops::ShooterChangeCalibration(
- encoder_val, known_position, old_position,
- absolute_position(), previous_offset, offset_));
}
ShooterMotor::ShooterMotor(::aos::EventLoop *event_loop,
const ::std::string &name)
- : aos::controls::ControlLoop<::y2014::control_loops::ShooterQueue>(
- event_loop, name),
+ : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name),
shooter_(MakeShooterLoop()),
state_(STATE_INITIALIZE),
cycles_not_moved_(0),
@@ -137,12 +122,8 @@
(kMaxExtension - values.shooter.upper_limit) *
(kMaxExtension - values.shooter.upper_limit));
if (power < 0) {
- AOS_LOG_STRUCT(WARNING, "negative power",
- ::y2014::control_loops::PowerAdjustment(power, 0));
power = 0;
} else if (power > maxpower) {
- AOS_LOG_STRUCT(WARNING, "power too high",
- ::y2014::control_loops::PowerAdjustment(power, maxpower));
power = maxpower;
}
@@ -166,23 +147,23 @@
return power;
}
-void ShooterMotor::CheckCalibrations(
- const ::y2014::control_loops::ShooterQueue::Position *position) {
+void ShooterMotor::CheckCalibrations(const Position *position) {
AOS_CHECK_NOTNULL(position);
const constants::Values &values = constants::GetValues();
// TODO(austin): Validate that this is the right edge.
// If we see a posedge on any of the hall effects,
- if (position->pusher_proximal.posedge_count != last_proximal_posedge_count_ &&
+ if (position->pusher_proximal()->posedge_count() !=
+ last_proximal_posedge_count_ &&
!last_proximal_current_) {
proximal_posedge_validation_cycles_left_ = 2;
}
if (proximal_posedge_validation_cycles_left_ > 0) {
- if (position->pusher_proximal.current) {
+ if (position->pusher_proximal()->current()) {
--proximal_posedge_validation_cycles_left_;
if (proximal_posedge_validation_cycles_left_ == 0) {
shooter_.SetCalibration(
- position->pusher_proximal.posedge_value,
+ position->pusher_proximal()->posedge_value(),
values.shooter.pusher_proximal.upper_angle);
AOS_LOG(DEBUG, "Setting calibration using proximal sensor\n");
@@ -193,16 +174,17 @@
}
}
- if (position->pusher_distal.posedge_count != last_distal_posedge_count_ &&
+ if (position->pusher_distal()->posedge_count() !=
+ last_distal_posedge_count_ &&
!last_distal_current_) {
distal_posedge_validation_cycles_left_ = 2;
}
if (distal_posedge_validation_cycles_left_ > 0) {
- if (position->pusher_distal.current) {
+ if (position->pusher_distal()->current()) {
--distal_posedge_validation_cycles_left_;
if (distal_posedge_validation_cycles_left_ == 0) {
shooter_.SetCalibration(
- position->pusher_distal.posedge_value,
+ position->pusher_distal()->posedge_value(),
values.shooter.pusher_distal.upper_angle);
AOS_LOG(DEBUG, "Setting calibration using distal sensor\n");
@@ -216,43 +198,44 @@
// Positive is out, and positive power is out.
void ShooterMotor::RunIteration(
- const ::y2014::control_loops::ShooterQueue::Goal *goal,
- const ::y2014::control_loops::ShooterQueue::Position *position,
- ::y2014::control_loops::ShooterQueue::Output *output,
- ::y2014::control_loops::ShooterQueue::Status *status) {
+ const Goal *goal,
+ const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) {
+ OutputT output_struct;
const monotonic_clock::time_point monotonic_now = event_loop()->monotonic_now();
- if (goal && ::std::isnan(goal->shot_power)) {
+ if (goal && ::std::isnan(goal->shot_power())) {
state_ = STATE_ESTOP;
AOS_LOG(ERROR, "Estopping because got a shot power of NAN.\n");
}
// we must always have these or we have issues.
if (status == NULL) {
- if (output) output->voltage = 0;
+ if (output) output_struct.voltage = 0;
AOS_LOG(ERROR, "Thought I would just check for null and die.\n");
return;
}
- status->ready = false;
+ bool status_ready = false;
if (WasReset()) {
state_ = STATE_INITIALIZE;
- last_distal_current_ = position->pusher_distal.current;
- last_proximal_current_ = position->pusher_proximal.current;
+ last_distal_current_ = position->pusher_distal()->current();
+ last_proximal_current_ = position->pusher_proximal()->current();
}
if (position) {
- shooter_.CorrectPosition(position->position);
+ shooter_.CorrectPosition(position->position());
}
// Disable the motors now so that all early returns will return with the
// motors disabled.
- if (output) output->voltage = 0;
+ if (output) output_struct.voltage = 0;
const constants::Values &values = constants::GetValues();
// Don't even let the control loops run.
bool shooter_loop_disable = false;
- const bool disabled = !has_joystick_state() || !joystick_state().enabled;
+ const bool disabled = !has_joystick_state() || !joystick_state().enabled();
// If true, move the goal if we saturate.
bool cap_goal = false;
@@ -263,7 +246,7 @@
if (position) {
int last_index = shooter_.index();
- if (position->plunger && position->latch) {
+ if (position->plunger() && position->latch()) {
// Use the controller without the spring if the latch is set and the
// plunger is back
shooter_.set_index(1);
@@ -280,27 +263,27 @@
case STATE_INITIALIZE:
if (position) {
// Reinitialize the internal filter state.
- shooter_.InitializeState(position->position);
+ shooter_.InitializeState(position->position());
// Start off with the assumption that we are at the value
// futhest back given our sensors.
- if (position->pusher_distal.current) {
- shooter_.SetCalibration(position->position,
+ if (position->pusher_distal()->current()) {
+ shooter_.SetCalibration(position->position(),
values.shooter.pusher_distal.lower_angle);
- } else if (position->pusher_proximal.current) {
- shooter_.SetCalibration(position->position,
+ } else if (position->pusher_proximal()->current()) {
+ shooter_.SetCalibration(position->position(),
values.shooter.pusher_proximal.upper_angle);
} else {
- shooter_.SetCalibration(position->position,
+ shooter_.SetCalibration(position->position(),
values.shooter.upper_limit);
}
// Go to the current position.
shooter_.SetGoalPosition(shooter_.absolute_position(), 0.0);
// If the plunger is all the way back, we want to be latched.
- latch_piston_ = position->plunger;
+ latch_piston_ = position->plunger();
brake_piston_ = false;
- if (position->latch == latch_piston_) {
+ if (position->latch() == latch_piston_) {
state_ = STATE_REQUEST_LOAD;
} else {
shooter_loop_disable = true;
@@ -317,20 +300,20 @@
case STATE_REQUEST_LOAD:
if (position) {
zeroed_ = false;
- if (position->pusher_distal.current ||
- position->pusher_proximal.current) {
+ if (position->pusher_distal()->current() ||
+ position->pusher_proximal()->current()) {
// We started on the sensor, back up until we are found.
// If the plunger is all the way back and not latched, it won't be
// there for long.
state_ = STATE_LOAD_BACKTRACK;
// The plunger is already back and latched. Don't release it.
- if (position->plunger && position->latch) {
+ if (position->plunger() && position->latch()) {
latch_piston_ = true;
} else {
latch_piston_ = false;
}
- } else if (position->plunger && position->latch) {
+ } else if (position->plunger() && position->latch()) {
// The plunger is back and we are latched. We most likely got here
// from Initialize, in which case we want to 'load' again anyways to
// zero.
@@ -361,11 +344,11 @@
shooter_.set_max_voltage(4.0);
if (position) {
- if (!position->pusher_distal.current &&
- !position->pusher_proximal.current) {
+ if (!position->pusher_distal()->current() &&
+ !position->pusher_proximal()->current()) {
Load(monotonic_now);
}
- latch_piston_ = position->plunger;
+ latch_piston_ = position->plunger();
}
brake_piston_ = false;
@@ -391,18 +374,19 @@
// Latch if the plunger is far enough back to trigger the hall effect.
// This happens when the distal sensor is triggered.
- latch_piston_ = position->pusher_distal.current || position->plunger;
+ latch_piston_ =
+ position->pusher_distal()->current() || position->plunger();
// Check if we are latched and back. Make sure the plunger is all the
// way back as well.
- if (position->plunger && position->latch &&
- position->pusher_distal.current) {
+ if (position->plunger() && position->latch() &&
+ position->pusher_distal()->current()) {
if (!zeroed_) {
state_ = STATE_REQUEST_LOAD;
} else {
state_ = STATE_PREPARE_SHOT;
}
- } else if (position->plunger &&
+ } else if (position->plunger() &&
::std::abs(shooter_.absolute_position() -
shooter_.goal_position()) < 0.001) {
// We are at the goal, but not latched.
@@ -415,10 +399,9 @@
if (position) {
// If none of the sensors is triggered, estop.
// Otherwise, trigger anyways if it has been 0.5 seconds more.
- if (!(position->pusher_distal.current ||
- position->pusher_proximal.current) ||
- (load_timeout_ + chrono::milliseconds(500) <
- monotonic_now)) {
+ if (!(position->pusher_distal()->current() ||
+ position->pusher_proximal()->current()) ||
+ (load_timeout_ + chrono::milliseconds(500) < monotonic_now)) {
state_ = STATE_ESTOP;
AOS_LOG(ERROR, "Estopping because took too long to load.\n");
}
@@ -436,7 +419,7 @@
if (monotonic_now > loading_problem_end_time_) {
// Timeout by unloading.
Unload(monotonic_now);
- } else if (position && position->plunger && position->latch) {
+ } else if (position && position->plunger() && position->latch()) {
// If both trigger, we are latched.
state_ = STATE_PREPARE_SHOT;
}
@@ -448,7 +431,7 @@
shooter_.SetGoalPosition(values.shooter.lower_limit, 0.0);
if (position) {
AOS_LOG(DEBUG, "Waiting on latch: plunger %d, latch: %d\n",
- position->plunger, position->latch);
+ position->plunger(), position->latch());
}
latch_piston_ = true;
@@ -459,16 +442,16 @@
// TODO(austin): Timeout. Low priority.
if (goal) {
- shooter_.SetGoalPosition(PowerToPosition(goal->shot_power), 0.0);
+ shooter_.SetGoalPosition(PowerToPosition(goal->shot_power()), 0.0);
}
AOS_LOG(DEBUG, "PDIFF: absolute_position: %.2f, pow: %.2f\n",
shooter_.absolute_position(),
- goal ? PowerToPosition(goal->shot_power)
+ goal ? PowerToPosition(goal->shot_power())
: ::std::numeric_limits<double>::quiet_NaN());
if (goal &&
::std::abs(shooter_.absolute_position() -
- PowerToPosition(goal->shot_power)) < 0.001 &&
+ PowerToPosition(goal->shot_power())) < 0.001 &&
::std::abs(shooter_.absolute_velocity()) < 0.005) {
// We are there, set the brake and move on.
latch_piston_ = true;
@@ -479,7 +462,7 @@
latch_piston_ = true;
brake_piston_ = false;
}
- if (goal && goal->unload_requested) {
+ if (goal && goal->unload_requested()) {
Unload(monotonic_now);
}
break;
@@ -488,12 +471,12 @@
// Wait until the brake is set, and a shot is requested or the shot power
// is changed.
if (monotonic_now > shooter_brake_set_time_) {
- status->ready = true;
+ status_ready = true;
// We have waited long enough for the brake to set, turn the shooter
// control loop off.
shooter_loop_disable = true;
AOS_LOG(DEBUG, "Brake is now set\n");
- if (goal && goal->shot_requested && !disabled) {
+ if (goal && goal->shot_requested() && !disabled) {
AOS_LOG(DEBUG, "Shooting now\n");
shooter_loop_disable = true;
shot_end_time_ = monotonic_now + kShotEndTimeout;
@@ -503,24 +486,24 @@
}
if (state_ == STATE_READY && goal &&
::std::abs(shooter_.absolute_position() -
- PowerToPosition(goal->shot_power)) > 0.002) {
+ PowerToPosition(goal->shot_power())) > 0.002) {
// TODO(austin): Add a state to release the brake.
// TODO(austin): Do we want to set the brake here or after shooting?
// Depends on air usage.
- status->ready = false;
+ status_ready = false;
AOS_LOG(DEBUG, "Preparing shot again.\n");
state_ = STATE_PREPARE_SHOT;
}
if (goal) {
- shooter_.SetGoalPosition(PowerToPosition(goal->shot_power), 0.0);
+ shooter_.SetGoalPosition(PowerToPosition(goal->shot_power()), 0.0);
}
latch_piston_ = true;
brake_piston_ = true;
- if (goal && goal->unload_requested) {
+ if (goal && goal->unload_requested()) {
Unload(monotonic_now);
}
break;
@@ -528,7 +511,7 @@
case STATE_FIRE:
if (disabled) {
if (position) {
- if (position->plunger) {
+ if (position->plunger()) {
// If disabled and the plunger is still back there, reset the
// timeout.
shot_end_time_ = monotonic_now + kShotEndTimeout;
@@ -537,7 +520,7 @@
}
shooter_loop_disable = true;
// Count the number of contiguous cycles during which we haven't moved.
- if (::std::abs(last_position_.position - shooter_.absolute_position()) <
+ if (::std::abs(last_position_position_ - shooter_.absolute_position()) <
0.0005) {
++cycles_not_moved_;
} else {
@@ -551,7 +534,7 @@
shooter_.absolute_position()) > 0.0005 &&
cycles_not_moved_ > 6) ||
monotonic_now > shot_end_time_) &&
- robot_state().voltage_battery > 10.5) {
+ robot_state().voltage_battery() > 10.5) {
state_ = STATE_REQUEST_LOAD;
++shot_count_;
}
@@ -566,9 +549,9 @@
// the plunger.
bool all_back;
if (position) {
- all_back = position->plunger && position->latch;
+ all_back = position->plunger() && position->latch();
} else {
- all_back = last_position_.plunger && last_position_.latch;
+ all_back = last_position_plunger_ && last_position_latch_;
}
if (all_back) {
@@ -632,7 +615,7 @@
brake_piston_ = false;
} break;
case STATE_READY_UNLOAD:
- if (goal && goal->load_requested) {
+ if (goal && goal->load_requested()) {
state_ = STATE_REQUEST_LOAD;
}
// If we are ready to load again,
@@ -652,9 +635,6 @@
}
if (!shooter_loop_disable) {
- AOS_LOG_STRUCT(DEBUG, "running the loop",
- ::y2014::control_loops::ShooterStatusToLog(
- shooter_.goal_position(), shooter_.absolute_position()));
if (!cap_goal) {
shooter_.set_max_voltage(12.0);
}
@@ -664,49 +644,55 @@
}
// We don't really want to output anything if we went through everything
// assuming the motors weren't working.
- if (output) output->voltage = shooter_.voltage();
+ if (output) output_struct.voltage = shooter_.voltage();
} else {
shooter_.Update(true);
shooter_.ZeroPower();
- if (output) output->voltage = 0.0;
+ if (output) output_struct.voltage = 0.0;
}
- status->hard_stop_power = PositionToPower(shooter_.absolute_position());
+ Status::Builder status_builder = status->MakeBuilder<Status>();
+
+ status_builder.add_ready(status_ready);
+ status_builder.add_hard_stop_power(
+ PositionToPower(shooter_.absolute_position()));
if (output) {
- output->latch_piston = latch_piston_;
- output->brake_piston = brake_piston_;
+ output_struct.latch_piston = latch_piston_;
+ output_struct.brake_piston = brake_piston_;
+
+ output->Send(Output::Pack(*output->fbb(), &output_struct));
}
if (position) {
- AOS_LOG_STRUCT(
- DEBUG, "internal state",
- ::y2014::control_loops::ShooterStateToLog(
- shooter_.absolute_position(), shooter_.absolute_velocity(), state_,
- position->latch, position->pusher_proximal.current,
- position->pusher_distal.current, position->plunger, brake_piston_,
- latch_piston_));
+ last_position_latch_ = position->latch();
+ last_position_plunger_ = position->plunger();
+ last_position_position_ = position->position();
- last_position_ = *position;
-
- last_distal_posedge_count_ = position->pusher_distal.posedge_count;
- last_proximal_posedge_count_ = position->pusher_proximal.posedge_count;
- last_distal_current_ = position->pusher_distal.current;
- last_proximal_current_ = position->pusher_proximal.current;
+ last_distal_posedge_count_ = position->pusher_distal()->posedge_count();
+ last_proximal_posedge_count_ = position->pusher_proximal()->posedge_count();
+ last_distal_current_ = position->pusher_distal()->current();
+ last_proximal_current_ = position->pusher_proximal()->current();
}
- status->absolute_position = shooter_.absolute_position();
- status->absolute_velocity = shooter_.absolute_velocity();
- status->state = state_;
+ status_builder.add_absolute_position(shooter_.absolute_position());
+ status_builder.add_absolute_velocity(shooter_.absolute_velocity());
+ status_builder.add_state(state_);
- status->shots = shot_count_;
+ status_builder.add_shots(shot_count_);
+
+ status->Send(status_builder.Finish());
}
-void ShooterMotor::Zero(::y2014::control_loops::ShooterQueue::Output *output) {
- output->voltage = 0.0;
- output->latch_piston = latch_piston_;
- output->brake_piston = brake_piston_;
+flatbuffers::Offset<Output> ShooterMotor::Zero(
+ aos::Sender<Output>::Builder *output) {
+ Output::Builder output_builder = output->MakeBuilder<Output>();
+ output_builder.add_voltage(0.0);
+ output_builder.add_latch_piston(latch_piston_);
+ output_builder.add_brake_piston(brake_piston_);
+ return output_builder.Finish();
}
+} // namespace shooter
} // namespace control_loops
} // namespace y2014
diff --git a/y2014/control_loops/shooter/shooter.h b/y2014/control_loops/shooter/shooter.h
index e755b0f..1e7d4aa 100644
--- a/y2014/control_loops/shooter/shooter.h
+++ b/y2014/control_loops/shooter/shooter.h
@@ -8,11 +8,15 @@
#include "aos/time/time.h"
#include "y2014/constants.h"
+#include "y2014/control_loops/shooter/shooter_goal_generated.h"
#include "y2014/control_loops/shooter/shooter_motor_plant.h"
-#include "y2014/control_loops/shooter/shooter.q.h"
+#include "y2014/control_loops/shooter/shooter_output_generated.h"
+#include "y2014/control_loops/shooter/shooter_position_generated.h"
+#include "y2014/control_loops/shooter/shooter_status_generated.h"
namespace y2014 {
namespace control_loops {
+namespace shooter {
namespace testing {
class ShooterTest_UnloadWindupPositive_Test;
class ShooterTest_UnloadWindupNegative_Test;
@@ -127,19 +131,17 @@
::std::chrono::milliseconds(40);
class ShooterMotor
- : public aos::controls::ControlLoop<::y2014::control_loops::ShooterQueue> {
+ : public aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
- explicit ShooterMotor(
- ::aos::EventLoop *event_loop,
- const ::std::string &name = ".y2014.control_loops.shooter_queue");
+ explicit ShooterMotor(::aos::EventLoop *event_loop,
+ const ::std::string &name = "/shooter");
// True if the goal was moved to avoid goal windup.
bool capped_goal() const { return shooter_.capped_goal(); }
double PowerToPosition(double power);
double PositionToPower(double position);
- void CheckCalibrations(
- const ::y2014::control_loops::ShooterQueue::Position *position);
+ void CheckCalibrations(const Position *position);
typedef enum {
STATE_INITIALIZE = 0,
@@ -159,15 +161,14 @@
State state() { return state_; }
protected:
- void RunIteration(
- const ::y2014::control_loops::ShooterQueue::Goal *goal,
- const ::y2014::control_loops::ShooterQueue::Position *position,
- ::y2014::control_loops::ShooterQueue::Output *output,
- ::y2014::control_loops::ShooterQueue::Status *status) override;
+ void RunIteration(const Goal *goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) override;
private:
// We have to override this to keep the pistons in the correct positions.
- void Zero(::y2014::control_loops::ShooterQueue::Output *output) override;
+ flatbuffers::Offset<Output> Zero(
+ aos::Sender<Output>::Builder *output) override;
// Friend the test classes for acces to the internal state.
friend class testing::ShooterTest_UnloadWindupPositive_Test;
@@ -185,7 +186,9 @@
load_timeout_ = monotonic_now + kLoadTimeout;
}
- ::y2014::control_loops::ShooterQueue::Position last_position_;
+ bool last_position_latch_ = false;
+ bool last_position_plunger_ = false;
+ double last_position_position_ = 0.0;
ZeroedStateFeedbackLoop shooter_;
@@ -232,6 +235,7 @@
DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
};
+} // namespace shooter
} // namespace control_loops
} // namespace y2014
diff --git a/y2014/control_loops/shooter/shooter.q b/y2014/control_loops/shooter/shooter.q
deleted file mode 100644
index c7ed97d..0000000
--- a/y2014/control_loops/shooter/shooter.q
+++ /dev/null
@@ -1,103 +0,0 @@
-package y2014.control_loops;
-
-import "aos/controls/control_loops.q";
-import "frc971/control_loops/control_loops.q";
-
-// Published on ".y2014.control_loops.shooter_queue"
-queue_group ShooterQueue {
- implements aos.control_loops.ControlLoop;
-
- message Output {
- double voltage;
- // true: latch engaged, false: latch open
- bool latch_piston;
- // true: brake engaged false: brake released
- bool brake_piston;
- };
- message Goal {
- // Shot power in joules.
- double shot_power;
- // Shoots as soon as this is true.
- bool shot_requested;
- bool unload_requested;
- bool load_requested;
- };
-
- // Back is when the springs are all the way stretched.
- message Position {
- // In meters, out is positive.
- double position;
-
- // If the latch piston is fired and this hall effect has been triggered, the
- // plunger is all the way back and latched.
- bool plunger;
- // Gets triggered when the pusher is all the way back.
- .frc971.PosedgeOnlyCountedHallEffectStruct pusher_distal;
- // Triggers just before pusher_distal.
- .frc971.PosedgeOnlyCountedHallEffectStruct pusher_proximal;
- // Triggers when the latch engages.
- bool latch;
- };
- message Status {
- // Whether it's ready to shoot right now.
- bool ready;
- // Whether the plunger is in and out of the way of grabbing a ball.
- // TODO(ben): Populate these!
- bool cocked;
- // How many times we've shot.
- int32_t shots;
- bool done;
- // What we think the current position of the hard stop on the shooter is, in
- // shot power (Joules).
- double hard_stop_power;
-
- double absolute_position;
- double absolute_velocity;
- uint32_t state;
- };
-
- queue Goal goal;
- queue Position position;
- queue Output output;
- queue Status status;
-};
-
-struct ShooterStateToLog {
- double absolute_position;
- double absolute_velocity;
- uint32_t state;
- bool latch_sensor;
- bool proximal;
- bool distal;
- bool plunger;
- bool brake;
- bool latch_piston;
-};
-
-struct ShooterVoltageToLog {
- double X_hat;
- double applied;
-};
-
-struct ShooterMovingGoal {
- double dx;
-};
-
-struct ShooterChangeCalibration {
- double encoder;
- double real_position;
- double old_position;
- double new_position;
- double old_offset;
- double new_offset;
-};
-
-struct ShooterStatusToLog {
- double goal;
- double position;
-};
-
-struct PowerAdjustment {
- double requested_power;
- double actual_power;
-};
diff --git a/y2014/control_loops/shooter/shooter_goal.fbs b/y2014/control_loops/shooter/shooter_goal.fbs
new file mode 100644
index 0000000..3326562
--- /dev/null
+++ b/y2014/control_loops/shooter/shooter_goal.fbs
@@ -0,0 +1,12 @@
+namespace y2014.control_loops.shooter;
+
+table Goal {
+ // Shot power in joules.
+ shot_power:double;
+ // Shoots as soon as this is true.
+ shot_requested:bool;
+ unload_requested:bool;
+ load_requested:bool;
+}
+
+root_type Goal;
diff --git a/y2014/control_loops/shooter/shooter_lib_test.cc b/y2014/control_loops/shooter/shooter_lib_test.cc
index f910b6d..ad24cf9 100644
--- a/y2014/control_loops/shooter/shooter_lib_test.cc
+++ b/y2014/control_loops/shooter/shooter_lib_test.cc
@@ -9,7 +9,10 @@
#include "gtest/gtest.h"
#include "y2014/constants.h"
#include "y2014/control_loops/shooter/shooter.h"
-#include "y2014/control_loops/shooter/shooter.q.h"
+#include "y2014/control_loops/shooter/shooter_goal_generated.h"
+#include "y2014/control_loops/shooter/shooter_output_generated.h"
+#include "y2014/control_loops/shooter/shooter_position_generated.h"
+#include "y2014/control_loops/shooter/shooter_status_generated.h"
#include "y2014/control_loops/shooter/unaugmented_shooter_motor_plant.h"
namespace chrono = ::std::chrono;
@@ -17,6 +20,7 @@
namespace y2014 {
namespace control_loops {
+namespace shooter {
namespace testing {
using ::y2014::control_loops::shooter::kMaxExtension;
@@ -31,11 +35,8 @@
ShooterSimulation(::aos::EventLoop *event_loop, chrono::nanoseconds dt,
double initial_position)
: event_loop_(event_loop),
- shooter_position_sender_(
- event_loop_->MakeSender<ShooterQueue::Position>(
- ".y2014.control_loops.shooter_queue.position")),
- shooter_output_fetcher_(event_loop_->MakeFetcher<ShooterQueue::Output>(
- ".y2014.control_loops.shooter_queue.output")),
+ shooter_position_sender_(event_loop_->MakeSender<Position>("/shooter")),
+ shooter_output_fetcher_(event_loop_->MakeFetcher<Output>("/shooter")),
shooter_plant_(new StateFeedbackPlant<2, 1, 1>(MakeRawShooterPlant())),
latch_piston_state_(false),
latch_delay_count_(0),
@@ -69,6 +70,7 @@
plant->mutable_Y() = plant->C() * plant->X();
last_voltage_ = 0.0;
last_plant_position_ = 0.0;
+ last_position_ = 0.0;
SetPhysicalSensors(&last_position_message_);
}
@@ -89,8 +91,7 @@
// Sets the values of the physical sensors that can be directly observed
// (encoder, hall effect).
- void SetPhysicalSensors(
- ::y2014::control_loops::ShooterQueue::Position *position) {
+ void SetPhysicalSensors(PositionT *position) {
const constants::Values &values = constants::GetValues();
position->position = GetPosition();
@@ -113,17 +114,27 @@
position->plunger =
CheckRange(GetAbsolutePosition(), values.shooter.plunger_back);
}
- position->pusher_distal.current =
+
+ if (!position->pusher_distal) {
+ position->pusher_distal.reset(
+ new frc971::PosedgeOnlyCountedHallEffectStructT);
+ }
+ if (!position->pusher_proximal) {
+ position->pusher_proximal.reset(
+ new frc971::PosedgeOnlyCountedHallEffectStructT);
+ }
+
+ position->pusher_distal->current =
CheckRange(GetAbsolutePosition(), values.shooter.pusher_distal);
- position->pusher_proximal.current =
+ position->pusher_proximal->current =
CheckRange(GetAbsolutePosition(), values.shooter.pusher_proximal);
}
void UpdateEffectEdge(
- ::frc971::PosedgeOnlyCountedHallEffectStruct *sensor,
- const ::frc971::PosedgeOnlyCountedHallEffectStruct &last_sensor,
+ ::frc971::PosedgeOnlyCountedHallEffectStructT *sensor,
+ const ::frc971::PosedgeOnlyCountedHallEffectStructT &last_sensor,
const constants::Values::AnglePair &limits,
- const ::y2014::control_loops::ShooterQueue::Position &last_position) {
+ float last_position) {
sensor->posedge_count = last_sensor.posedge_count;
sensor->negedge_count = last_sensor.negedge_count;
@@ -131,7 +142,7 @@
if (sensor->current && !last_sensor.current) {
++sensor->posedge_count;
- if (last_position.position + initial_position_ < limits.lower_angle) {
+ if (last_position + initial_position_ < limits.lower_angle) {
AOS_LOG(DEBUG,
"Posedge value on lower edge of sensor, count is now %d\n",
sensor->posedge_count);
@@ -159,8 +170,15 @@
// it into a state using the plunger_in_, brake_in_, and latch_in_ values.
void SendPositionMessage() {
const constants::Values &values = constants::GetValues();
- ::aos::Sender<ShooterQueue::Position>::Message position =
- shooter_position_sender_.MakeMessage();
+ ::aos::Sender<Position>::Builder builder =
+ shooter_position_sender_.MakeBuilder();
+
+ PositionT position;
+
+ position.pusher_distal.reset(
+ new frc971::PosedgeOnlyCountedHallEffectStructT);
+ position.pusher_proximal.reset(
+ new frc971::PosedgeOnlyCountedHallEffectStructT);
if (use_passed_) {
plunger_latched_ = latch_in_ && plunger_in_;
@@ -168,43 +186,45 @@
brake_piston_state_ = brake_in_;
}
- SetPhysicalSensors(position.get());
+ SetPhysicalSensors(&position);
- position->latch = latch_piston_state_;
+ position.latch = latch_piston_state_;
// Handle pusher distal hall effect
- UpdateEffectEdge(&position->pusher_distal,
- last_position_message_.pusher_distal,
- values.shooter.pusher_distal, last_position_message_);
+ UpdateEffectEdge(position.pusher_distal.get(),
+ *last_position_message_.pusher_distal.get(),
+ values.shooter.pusher_distal, last_position_);
// Handle pusher proximal hall effect
- UpdateEffectEdge(&position->pusher_proximal,
- last_position_message_.pusher_proximal,
- values.shooter.pusher_proximal, last_position_message_);
+ UpdateEffectEdge(position.pusher_proximal.get(),
+ *last_position_message_.pusher_proximal.get(),
+ values.shooter.pusher_proximal, last_position_);
- last_position_message_ = *position;
- position.Send();
+ builder.Send(Position::Pack(*builder.fbb(), &position));
+
+ last_position_ = position.position;
+ last_position_message_ = std::move(position);
}
// Simulates the claw moving for one timestep.
void Simulate() {
last_plant_position_ = GetAbsolutePosition();
EXPECT_TRUE(shooter_output_fetcher_.Fetch());
- if (shooter_output_fetcher_->latch_piston && !latch_piston_state_ &&
+ if (shooter_output_fetcher_->latch_piston() && !latch_piston_state_ &&
latch_delay_count_ <= 0) {
ASSERT_EQ(0, latch_delay_count_) << "The test doesn't support that.";
latch_delay_count_ = 6;
- } else if (!shooter_output_fetcher_->latch_piston &&
+ } else if (!shooter_output_fetcher_->latch_piston() &&
latch_piston_state_ && latch_delay_count_ >= 0) {
ASSERT_EQ(0, latch_delay_count_) << "The test doesn't support that.";
latch_delay_count_ = -6;
}
- if (shooter_output_fetcher_->brake_piston && !brake_piston_state_ &&
+ if (shooter_output_fetcher_->brake_piston() && !brake_piston_state_ &&
brake_delay_count_ <= 0) {
ASSERT_EQ(0, brake_delay_count_) << "The test doesn't support that.";
brake_delay_count_ = 5;
- } else if (!shooter_output_fetcher_->brake_piston &&
+ } else if (!shooter_output_fetcher_->brake_piston() &&
brake_piston_state_ && brake_delay_count_ >= 0) {
ASSERT_EQ(0, brake_delay_count_) << "The test doesn't support that.";
brake_delay_count_ = -5;
@@ -265,13 +285,13 @@
EXPECT_LE(constants::GetValues().shooter.lower_hard_limit,
GetAbsolutePosition());
- last_voltage_ = shooter_output_fetcher_->voltage;
+ last_voltage_ = shooter_output_fetcher_->voltage();
}
private:
::aos::EventLoop *event_loop_;
- ::aos::Sender<ShooterQueue::Position> shooter_position_sender_;
- ::aos::Fetcher<ShooterQueue::Output> shooter_output_fetcher_;
+ ::aos::Sender<Position> shooter_position_sender_;
+ ::aos::Fetcher<Output> shooter_output_fetcher_;
bool first_ = true;
@@ -300,7 +320,8 @@
double initial_position_;
double last_voltage_;
- ShooterQueue::Position last_position_message_;
+ double last_position_ = 0.0;
+ PositionT last_position_message_;
double last_plant_position_;
};
@@ -310,13 +331,12 @@
protected:
ShooterTestTemplated()
: ::aos::testing::ControlLoopTestTemplated<TestType>(
+ aos::configuration::ReadConfig("y2014/config.json"),
// TODO(austin): I think this runs at 5 ms in real life.
chrono::microseconds(5000)),
test_event_loop_(this->MakeEventLoop()),
- shooter_goal_fetcher_(test_event_loop_->MakeFetcher<ShooterQueue::Goal>(
- ".y2014.control_loops.shooter_queue.goal")),
- shooter_goal_sender_(test_event_loop_->MakeSender<ShooterQueue::Goal>(
- ".y2014.control_loops.shooter_queue.goal")),
+ shooter_goal_fetcher_(test_event_loop_->MakeFetcher<Goal>("/shooter")),
+ shooter_goal_sender_(test_event_loop_->MakeSender<Goal>("/shooter")),
shooter_event_loop_(this->MakeEventLoop()),
shooter_motor_(shooter_event_loop_.get()),
shooter_plant_event_loop_(this->MakeEventLoop()),
@@ -330,12 +350,12 @@
void VerifyNearGoal() {
shooter_goal_fetcher_.Fetch();
const double pos = shooter_motor_plant_.GetAbsolutePosition();
- EXPECT_NEAR(shooter_goal_fetcher_->shot_power, pos, 1e-4);
+ EXPECT_NEAR(shooter_goal_fetcher_->shot_power(), pos, 1e-4);
}
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
- ::aos::Fetcher<ShooterQueue::Goal> shooter_goal_fetcher_;
- ::aos::Sender<ShooterQueue::Goal> shooter_goal_sender_;
+ ::aos::Fetcher<Goal> shooter_goal_fetcher_;
+ ::aos::Sender<Goal> shooter_goal_sender_;
// Create a loop and simulation plant.
::std::unique_ptr<::aos::EventLoop> shooter_event_loop_;
@@ -381,18 +401,18 @@
TEST_F(ShooterTest, GoesToValue) {
SetEnabled(true);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(2));
double pos = shooter_motor_plant_.GetAbsolutePosition();
EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
EXPECT_NEAR(
- shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
- pos, 0.05);
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power()), pos,
+ 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
@@ -400,19 +420,19 @@
TEST_F(ShooterTest, Fire) {
SetEnabled(true);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(1200));
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 35.0;
- message->shot_requested = true;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(35.0);
+ goal_builder.add_shot_requested(true);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
bool hit_fire = false;
@@ -421,11 +441,12 @@
RunFor(dt());
if (shooter_motor_.state() == ShooterMotor::STATE_FIRE) {
if (!hit_fire) {
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 17.0;
- message->shot_requested = false;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder =
+ shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(17.0);
+ goal_builder.add_shot_requested(false);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
hit_fire = true;
}
@@ -433,8 +454,9 @@
double pos = shooter_motor_plant_.GetAbsolutePosition();
EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
- EXPECT_NEAR(shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
- pos, 0.05);
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power()), pos,
+ 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
EXPECT_TRUE(hit_fire);
}
@@ -443,20 +465,20 @@
TEST_F(ShooterTest, FireLong) {
SetEnabled(true);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(1500));
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 0.0;
- message->shot_requested = true;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(0.0);
+ goal_builder.add_shot_requested(true);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
bool hit_fire = false;
@@ -465,10 +487,11 @@
RunFor(dt());
if (shooter_motor_.state() == ShooterMotor::STATE_FIRE) {
if (!hit_fire) {
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_requested = false;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder =
+ shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_requested(false);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
hit_fire = true;
}
@@ -476,8 +499,9 @@
double pos = shooter_motor_plant_.GetAbsolutePosition();
EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
- EXPECT_NEAR(shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
- pos, 0.05);
+ EXPECT_NEAR(
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power()), pos,
+ 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
EXPECT_TRUE(hit_fire);
}
@@ -487,10 +511,10 @@
TEST_F(ShooterTest, LoadTooFar) {
SetEnabled(true);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 500.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(500.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
while (test_event_loop_->monotonic_now() <
monotonic_clock::time_point(chrono::milliseconds(1600))) {
@@ -505,19 +529,19 @@
TEST_F(ShooterTest, MoveGoal) {
SetEnabled(true);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(1500));
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 14.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(14.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(500));
@@ -525,8 +549,8 @@
double pos = shooter_motor_plant_.GetAbsolutePosition();
EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
EXPECT_NEAR(
- shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
- pos, 0.05);
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power()), pos,
+ 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
@@ -534,18 +558,18 @@
TEST_F(ShooterTest, Unload) {
SetEnabled(true);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(1500));
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->unload_requested = true;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_unload_requested(true);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
while (test_event_loop_->monotonic_now() <
@@ -563,10 +587,10 @@
TEST_F(ShooterTest, RezeroWhileUnloading) {
SetEnabled(true);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(1500));
@@ -576,10 +600,10 @@
RunFor(chrono::milliseconds(500));
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->unload_requested = true;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_unload_requested(true);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
while (test_event_loop_->monotonic_now() <
@@ -597,18 +621,18 @@
TEST_F(ShooterTest, UnloadWindupNegative) {
SetEnabled(true);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(1500));
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->unload_requested = true;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_unload_requested(true);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
int kicked_delay = 20;
@@ -640,18 +664,18 @@
TEST_F(ShooterTest, UnloadWindupPositive) {
SetEnabled(true);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(1500));
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->unload_requested = true;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_unload_requested(true);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
int kicked_delay = 20;
@@ -688,18 +712,18 @@
SetEnabled(true);
Reinitialize(HallEffectMiddle(constants::GetValues().shooter.pusher_distal));
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(2));
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
EXPECT_NEAR(
- shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
- pos, 0.05);
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power()), pos,
+ 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
@@ -710,18 +734,18 @@
Reinitialize(
HallEffectMiddle(constants::GetValues().shooter.pusher_proximal));
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 70.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(70.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(3));
// EXPECT_NEAR(0.0, shooter_motor_.GetPosition(), 0.01);
double pos = shooter_motor_plant_.GetAbsolutePosition();
EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
EXPECT_NEAR(
- shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
- pos, 0.05);
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power()), pos,
+ 0.05);
EXPECT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
@@ -741,10 +765,10 @@
bool initialized = false;
Reinitialize(start_pos);
{
- ::aos::Sender<ShooterQueue::Goal>::Message message =
- shooter_goal_sender_.MakeMessage();
- message->shot_power = 120.0;
- EXPECT_TRUE(message.Send());
+ ::aos::Sender<Goal>::Builder builder = shooter_goal_sender_.MakeBuilder();
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_shot_power(120.0);
+ EXPECT_TRUE(builder.Send(goal_builder.Finish()));
}
while (test_event_loop_->monotonic_now() <
monotonic_clock::time_point(chrono::seconds(2))) {
@@ -759,8 +783,8 @@
double pos = shooter_motor_plant_.GetAbsolutePosition();
EXPECT_TRUE(shooter_goal_fetcher_.Fetch());
EXPECT_NEAR(
- shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power),
- pos, 0.05);
+ shooter_motor_.PowerToPosition(shooter_goal_fetcher_->shot_power()), pos,
+ 0.05);
ASSERT_EQ(ShooterMotor::STATE_READY, shooter_motor_.state());
}
@@ -784,5 +808,6 @@
// TODO(austin): Test all the timeouts...
} // namespace testing
+} // namespace shooter
} // namespace control_loops
} // namespace y2014
diff --git a/y2014/control_loops/shooter/shooter_main.cc b/y2014/control_loops/shooter/shooter_main.cc
index 2da76c3..4161fdf 100644
--- a/y2014/control_loops/shooter/shooter_main.cc
+++ b/y2014/control_loops/shooter/shooter_main.cc
@@ -1,13 +1,16 @@
#include "y2014/control_loops/shooter/shooter.h"
-#include "aos/events/shm-event-loop.h"
+#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
int main() {
::aos::InitNRT(true);
- ::aos::ShmEventLoop event_loop;
- ::y2014::control_loops::ShooterMotor shooter(&event_loop);
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
+ ::y2014::control_loops::shooter::ShooterMotor shooter(&event_loop);
event_loop.Run();
diff --git a/y2014/control_loops/shooter/shooter_output.fbs b/y2014/control_loops/shooter/shooter_output.fbs
new file mode 100644
index 0000000..e1900c4
--- /dev/null
+++ b/y2014/control_loops/shooter/shooter_output.fbs
@@ -0,0 +1,11 @@
+namespace y2014.control_loops.shooter;
+
+table Output {
+ voltage:double;
+ // true: latch engaged, false: latch open
+ latch_piston:bool;
+ // true: brake engaged false: brake released
+ brake_piston:bool;
+}
+
+root_type Output;
diff --git a/y2014/control_loops/shooter/shooter_position.fbs b/y2014/control_loops/shooter/shooter_position.fbs
new file mode 100644
index 0000000..9c73a1a
--- /dev/null
+++ b/y2014/control_loops/shooter/shooter_position.fbs
@@ -0,0 +1,21 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2014.control_loops.shooter;
+
+// Back is when the springs are all the way stretched.
+table Position {
+ // In meters, out is positive.
+ position:double;
+
+ // If the latch piston is fired and this hall effect has been triggered, the
+ // plunger is all the way back and latched.
+ plunger:bool;
+ // Gets triggered when the pusher is all the way back.
+ pusher_distal:frc971.PosedgeOnlyCountedHallEffectStruct;
+ // Triggers just before pusher_distal.
+ pusher_proximal:frc971.PosedgeOnlyCountedHallEffectStruct;
+ // Triggers when the latch engages.
+ latch:bool;
+}
+
+root_type Position;
diff --git a/y2014/control_loops/shooter/shooter_status.fbs b/y2014/control_loops/shooter/shooter_status.fbs
new file mode 100644
index 0000000..4e76e27
--- /dev/null
+++ b/y2014/control_loops/shooter/shooter_status.fbs
@@ -0,0 +1,21 @@
+namespace y2014.control_loops.shooter;
+
+table Status {
+ // Whether it's ready to shoot right now.
+ ready:bool;
+ // Whether the plunger is in and out of the way of grabbing a ball.
+ // TODO(ben): Populate these!
+ //cocked:bool;
+ // How many times we've shot.
+ shots:int;
+ //done:bool;
+ // What we think the current position of the hard stop on the shooter is, in
+ // shot power (Joules).
+ hard_stop_power:double;
+
+ absolute_position:double;
+ absolute_velocity:double;
+ state:uint;
+}
+
+root_type Status;