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/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;