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/y2017/control_loops/superstructure/BUILD b/y2017/control_loops/superstructure/BUILD
index 5a00bfe..535475d 100644
--- a/y2017/control_loops/superstructure/BUILD
+++ b/y2017/control_loops/superstructure/BUILD
@@ -1,19 +1,49 @@
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 = "superstructure_queue",
+flatbuffer_cc_library(
+ name = "superstructure_goal_fbs",
srcs = [
- "superstructure.q",
+ "superstructure_goal.fbs",
],
- deps = [
- "//aos/controls:control_loop_queues",
- "//frc971/control_loops:profiled_subsystem_queue",
- "//frc971/control_loops:queues",
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
],
)
+flatbuffer_cc_library(
+ name = "superstructure_position_fbs",
+ srcs = [
+ "superstructure_position.fbs",
+ ],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "superstructure_status_fbs",
+ srcs = [
+ "superstructure_status.fbs",
+ ],
+ gen_reflections = 1,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ "//frc971/control_loops:profiled_subsystem_fbs_includes",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "superstructure_output_fbs",
+ srcs = [
+ "superstructure_output.fbs",
+ ],
+ gen_reflections = 1,
+)
+
cc_library(
name = "superstructure_lib",
srcs = [
@@ -23,10 +53,13 @@
"superstructure.h",
],
deps = [
- ":superstructure_queue",
+ ":superstructure_goal_fbs",
+ ":superstructure_output_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
":vision_distance_average",
"//aos/controls:control_loop",
- "//aos/events:event-loop",
+ "//aos/events:event_loop",
"//y2017:constants",
"//y2017/control_loops/superstructure/column",
"//y2017/control_loops/superstructure/hood",
@@ -40,11 +73,14 @@
srcs = [
"superstructure_lib_test.cc",
],
+ data = ["//y2017:config.json"],
deps = [
+ ":superstructure_goal_fbs",
":superstructure_lib",
- ":superstructure_queue",
+ ":superstructure_output_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
"//aos:math",
- "//aos:queues",
"//aos/controls:control_loop_test",
"//aos/testing:googletest",
"//aos/time",
@@ -64,9 +100,8 @@
],
deps = [
":superstructure_lib",
- ":superstructure_queue",
"//aos:init",
- "//aos/events:shm-event-loop",
+ "//aos/events:shm_event_loop",
],
)
@@ -79,11 +114,12 @@
"vision_time_adjuster.h",
],
deps = [
- ":superstructure_queue",
"//aos/containers:ring_buffer",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//y2017/control_loops/drivetrain:polydrivetrain_plants",
- "//y2017/vision:vision_queue",
+ "//y2017/vision:vision_fbs",
],
)
@@ -92,6 +128,7 @@
srcs = [
"vision_time_adjuster_test.cc",
],
+ data = ["//y2017:config.json"],
deps = [
":vision_time_adjuster",
"//aos/events:simulated_event_loop",
@@ -109,7 +146,7 @@
deps = [
"//aos/containers:ring_buffer",
"//aos/time",
- "//y2017/vision:vision_queue",
+ "//y2017/vision:vision_fbs",
],
)
@@ -120,6 +157,7 @@
],
deps = [
":vision_distance_average",
+ "//aos:flatbuffers",
"//aos/testing:googletest",
"//aos/time",
],
diff --git a/y2017/control_loops/superstructure/column/BUILD b/y2017/control_loops/superstructure/column/BUILD
index 694dd47..63dd98c 100644
--- a/y2017/control_loops/superstructure/column/BUILD
+++ b/y2017/control_loops/superstructure/column/BUILD
@@ -1,88 +1,89 @@
genrule(
- name = 'genrule_column',
- cmd = '$(location //y2017/control_loops/python:column) $(OUTS)',
- tools = [
- '//y2017/control_loops/python:column',
- ],
- outs = [
- 'column_plant.h',
- 'column_plant.cc',
- 'column_integral_plant.h',
- 'column_integral_plant.cc',
- 'stuck_column_integral_plant.h',
- 'stuck_column_integral_plant.cc',
- ],
+ name = "genrule_column",
+ outs = [
+ "column_plant.h",
+ "column_plant.cc",
+ "column_integral_plant.h",
+ "column_integral_plant.cc",
+ "stuck_column_integral_plant.h",
+ "stuck_column_integral_plant.cc",
+ ],
+ cmd = "$(location //y2017/control_loops/python:column) $(OUTS)",
+ tools = [
+ "//y2017/control_loops/python:column",
+ ],
)
cc_library(
- name = 'column_plants',
- visibility = ['//visibility:public'],
- srcs = [
- 'column_plant.cc',
- 'column_integral_plant.cc',
- 'stuck_column_integral_plant.cc',
- ],
- hdrs = [
- 'column_plant.h',
- 'column_integral_plant.h',
- 'stuck_column_integral_plant.h',
- ],
- deps = [
- '//frc971/control_loops:state_feedback_loop',
- ],
+ name = "column_plants",
+ srcs = [
+ "column_integral_plant.cc",
+ "column_plant.cc",
+ "stuck_column_integral_plant.cc",
+ ],
+ hdrs = [
+ "column_integral_plant.h",
+ "column_plant.h",
+ "stuck_column_integral_plant.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:state_feedback_loop",
+ ],
)
cc_library(
- name = 'column',
- visibility = ['//visibility:public'],
- srcs = [
- 'column.cc',
- ],
- hdrs = [
- 'column.h',
- ],
- deps = [
- ':column_plants',
- ':column_zeroing',
- '//aos/controls:control_loop',
- '//aos:math',
- '//frc971/control_loops:profiled_subsystem',
- '//y2017/control_loops/superstructure/intake:intake',
- '//y2017/control_loops/superstructure:superstructure_queue',
- '//y2017/control_loops/superstructure:vision_time_adjuster',
- '//y2017:constants',
- ],
+ name = "column",
+ srcs = [
+ "column.cc",
+ ],
+ hdrs = [
+ "column.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":column_plants",
+ ":column_zeroing",
+ "//aos:math",
+ "//aos/controls:control_loop",
+ "//frc971/control_loops:profiled_subsystem",
+ "//y2017:constants",
+ "//y2017/control_loops/superstructure:superstructure_position_fbs",
+ "//y2017/control_loops/superstructure:superstructure_status_fbs",
+ "//y2017/control_loops/superstructure:vision_time_adjuster",
+ "//y2017/control_loops/superstructure/intake",
+ ],
)
cc_library(
- name = 'column_zeroing',
- srcs = [
- 'column_zeroing.cc',
- ],
- hdrs = [
- 'column_zeroing.h',
- ],
- deps = [
- '//frc971/control_loops:queues',
- '//frc971/zeroing:wrap',
- '//frc971/zeroing:zeroing',
- '//frc971:constants',
- '//y2017/control_loops/superstructure:superstructure_queue',
- '//y2017:constants',
- ],
+ name = "column_zeroing",
+ srcs = [
+ "column_zeroing.cc",
+ ],
+ hdrs = [
+ "column_zeroing.h",
+ ],
+ deps = [
+ "//frc971:constants",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/zeroing",
+ "//frc971/zeroing:wrap",
+ "//y2017:constants",
+ "//y2017/control_loops/superstructure:superstructure_position_fbs",
+ "//y2017/control_loops/superstructure:superstructure_status_fbs",
+ ],
)
cc_test(
- name = 'column_zeroing_test',
- srcs = [
- 'column_zeroing_test.cc',
- ],
- deps = [
- ':column_zeroing',
- '//aos/testing:test_shm',
- '//frc971/control_loops:position_sensor_sim',
- '//frc971/control_loops:team_number_test_environment',
- '//y2017/control_loops/superstructure:superstructure_queue',
- '//y2017:constants',
- ],
+ name = "column_zeroing_test",
+ srcs = [
+ "column_zeroing_test.cc",
+ ],
+ deps = [
+ ":column_zeroing",
+ "//aos/testing:test_shm",
+ "//frc971/control_loops:position_sensor_sim",
+ "//frc971/control_loops:team_number_test_environment",
+ "//y2017:constants",
+ ],
)
diff --git a/y2017/control_loops/superstructure/column/column.cc b/y2017/control_loops/superstructure/column/column.cc
index ab1ff72..b2e7c5c 100644
--- a/y2017/control_loops/superstructure/column/column.cc
+++ b/y2017/control_loops/superstructure/column/column.cc
@@ -118,17 +118,18 @@
}
turret_last_position_ = turret_position();
- Y_ << new_position.indexer.encoder, new_position.turret.encoder;
+ Y_ << new_position.indexer()->encoder(), new_position.turret()->encoder();
Y_ += offset_;
loop_->Correct(Y_);
- indexer_history_[indexer_history_position_] = new_position.indexer.encoder;
+ indexer_history_[indexer_history_position_] =
+ new_position.indexer()->encoder();
indexer_history_position_ = (indexer_history_position_ + 1) % kHistoryLength;
indexer_dt_velocity_ =
- (new_position.indexer.encoder - indexer_last_position_) /
+ (new_position.indexer()->encoder() - indexer_last_position_) /
::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency);
- indexer_last_position_ = new_position.indexer.encoder;
+ indexer_last_position_ = new_position.indexer()->encoder();
stuck_indexer_detector_->Correct(Y_);
@@ -304,9 +305,11 @@
}
void ColumnProfiledSubsystem::AdjustProfile(
- const ::frc971::ProfileParameters &profile_parameters) {
- AdjustProfile(profile_parameters.max_velocity,
- profile_parameters.max_acceleration);
+ const ::frc971::ProfileParameters *profile_parameters) {
+ AdjustProfile(
+ profile_parameters == nullptr ? 0.0 : profile_parameters->max_velocity(),
+ profile_parameters == nullptr ? 0.0
+ : profile_parameters->max_acceleration());
}
void ColumnProfiledSubsystem::AdjustProfile(double max_angular_velocity,
@@ -348,20 +351,46 @@
stuck_indexer_detector_->mutable_X_hat(5, 0) = 0.0;
}
-void ColumnProfiledSubsystem::PopulateIndexerStatus(IndexerStatus *status) {
- status->avg_angular_velocity = indexer_average_angular_velocity_;
+void ColumnProfiledSubsystem::PopulateIndexerStatus(
+ IndexerStatus::Builder *status_builder) {
+ status_builder->add_avg_angular_velocity(indexer_average_angular_velocity_);
- status->angular_velocity = X_hat_current_(1, 0);
- status->ready = indexer_ready_;
+ status_builder->add_angular_velocity(X_hat_current_(1, 0));
+ status_builder->add_ready(indexer_ready_);
- status->voltage_error = X_hat_current_(4, 0);
- status->stuck_voltage_error = stuck_indexer_X_hat_current_(4, 0);
- status->position_error = indexer_position_error_;
- status->instantaneous_velocity = indexer_dt_velocity_;
+ status_builder->add_voltage_error(X_hat_current_(4, 0));
+ status_builder->add_stuck_voltage_error(stuck_indexer_X_hat_current_(4, 0));
+ status_builder->add_position_error(indexer_position_error_);
+ status_builder->add_instantaneous_velocity(indexer_dt_velocity_);
- status->stuck = IsIndexerStuck();
+ status_builder->add_stuck(IsIndexerStuck());
- status->stuck_voltage = IndexerStuckVoltage();
+ status_builder->add_stuck_voltage(IndexerStuckVoltage());
+}
+
+void ColumnProfiledSubsystem::PopulateTurretStatus(
+ TurretProfiledSubsystemStatus::Builder *status_builder,
+ flatbuffers::Offset<ColumnZeroingEstimator::State> estimator_state_offset) {
+ status_builder->add_zeroed(zeroed());
+
+ status_builder->add_position(X_hat(2, 0));
+ status_builder->add_velocity(X_hat(3, 0));
+ status_builder->add_goal_position(goal(2, 0));
+ status_builder->add_goal_velocity(goal(3, 0));
+ status_builder->add_unprofiled_goal_position(unprofiled_goal(2, 0));
+ status_builder->add_unprofiled_goal_velocity(unprofiled_goal(3, 0));
+ status_builder->add_voltage_error(X_hat(5, 0));
+ status_builder->add_calculated_velocity(
+ (turret_position() - turret_last_position_) /
+ ::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency));
+
+ status_builder->add_estimator_state(estimator_state_offset);
+
+ Eigen::Matrix<double, 6, 1> error = controller().error();
+ status_builder->add_position_power(controller().controller().K(0, 0) *
+ error(0, 0));
+ status_builder->add_velocity_power(controller().controller().K(0, 1) *
+ error(1, 0));
}
Column::Column(::aos::EventLoop *event_loop)
@@ -384,15 +413,15 @@
freeze_ = false;
}
-void Column::Iterate(const ::aos::monotonic_clock::time_point monotonic_now,
- const control_loops::IndexerGoal *unsafe_indexer_goal,
- const control_loops::TurretGoal *unsafe_turret_goal,
- const ColumnPosition *position,
- const vision::VisionStatus *vision_status,
- double *indexer_output, double *turret_output,
- IndexerStatus *indexer_status,
- TurretProfiledSubsystemStatus *turret_status,
- intake::Intake *intake) {
+std::pair<flatbuffers::Offset<IndexerStatus>,
+ flatbuffers::Offset<TurretProfiledSubsystemStatus>>
+Column::Iterate(const ::aos::monotonic_clock::time_point monotonic_now,
+ const IndexerGoalT *unsafe_indexer_goal,
+ const TurretGoal *unsafe_turret_goal,
+ const ColumnPosition *position,
+ const vision::VisionStatus *vision_status,
+ double *indexer_output, double *turret_output,
+ flatbuffers::FlatBufferBuilder *fbb, intake::Intake *intake) {
bool disable = turret_output == nullptr || indexer_output == nullptr;
profiled_subsystem_.Correct(*position);
@@ -526,11 +555,12 @@
}
if (unsafe_turret_goal && unsafe_indexer_goal) {
- profiled_subsystem_.AdjustProfile(unsafe_turret_goal->profile_params);
+ profiled_subsystem_.AdjustProfile(unsafe_turret_goal->profile_params());
profiled_subsystem_.set_unprofiled_goal(
- unsafe_indexer_goal->angular_velocity, unsafe_turret_goal->angle);
+ unsafe_indexer_goal->angular_velocity,
+ unsafe_turret_goal->angle());
- if (unsafe_turret_goal->track) {
+ if (unsafe_turret_goal->track()) {
if (vision_time_adjuster_.valid()) {
AOS_LOG(INFO, "Vision aligning to %f\n",
vision_time_adjuster_.goal());
@@ -623,23 +653,40 @@
// Save debug/internal state.
// TODO(austin): Save more.
- turret_status->zeroed = profiled_subsystem_.zeroed();
- profiled_subsystem_.PopulateTurretStatus(turret_status);
- turret_status->estopped = (state_ == State::ESTOP);
- turret_status->state = static_cast<int32_t>(state_);
- turret_status->turret_encoder_angle = profiled_subsystem_.turret_position();
+ flatbuffers::Offset<ColumnZeroingEstimator::State>
+ column_estimator_state_offset =
+ profiled_subsystem_.EstimatorState(fbb, 0);
+
+ TurretProfiledSubsystemStatus::Builder turret_status_builder(*fbb);
+ profiled_subsystem_.PopulateTurretStatus(&turret_status_builder,
+ column_estimator_state_offset);
+ turret_status_builder.add_estopped((state_ == State::ESTOP));
+ turret_status_builder.add_state(static_cast<int32_t>(state_));
+ turret_status_builder.add_turret_encoder_angle(
+ profiled_subsystem_.turret_position());
if (vision_time_adjuster_.valid()) {
- turret_status->vision_angle = vision_time_adjuster_.goal();
- turret_status->raw_vision_angle =
- vision_time_adjuster_.most_recent_vision_reading();
- turret_status->vision_tracking = true;
+ turret_status_builder.add_vision_angle(vision_time_adjuster_.goal());
+ turret_status_builder.add_raw_vision_angle(
+ vision_time_adjuster_.most_recent_vision_reading());
+ turret_status_builder.add_vision_tracking(true);
} else {
- turret_status->vision_angle = ::std::numeric_limits<double>::quiet_NaN();
- turret_status->vision_tracking = false;
+ turret_status_builder.add_vision_angle(
+ ::std::numeric_limits<double>::quiet_NaN());
+ turret_status_builder.add_vision_tracking(false);
}
- profiled_subsystem_.PopulateIndexerStatus(indexer_status);
- indexer_status->state = static_cast<int32_t>(indexer_state_);
+ flatbuffers::Offset<TurretProfiledSubsystemStatus> turret_status_offset =
+ turret_status_builder.Finish();
+
+ IndexerStatus::Builder indexer_status_builder(*fbb);
+ profiled_subsystem_.PopulateIndexerStatus(&indexer_status_builder);
+
+ indexer_status_builder.add_state(static_cast<int32_t>(indexer_state_));
+
+ flatbuffers::Offset<IndexerStatus> indexer_status_offset =
+ indexer_status_builder.Finish();
+
+ return std::make_pair(indexer_status_offset, turret_status_offset);
}
} // namespace column
diff --git a/y2017/control_loops/superstructure/column/column.h b/y2017/control_loops/superstructure/column/column.h
index 0382059..615565f 100644
--- a/y2017/control_loops/superstructure/column/column.h
+++ b/y2017/control_loops/superstructure/column/column.h
@@ -14,9 +14,10 @@
#include "y2017/constants.h"
#include "y2017/control_loops/superstructure/column/column_zeroing.h"
#include "y2017/control_loops/superstructure/intake/intake.h"
-#include "y2017/control_loops/superstructure/superstructure.q.h"
+#include "y2017/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2017/control_loops/superstructure/superstructure_status_generated.h"
#include "y2017/control_loops/superstructure/vision_time_adjuster.h"
-#include "y2017/vision/vision.q.h"
+#include "y2017/vision/vision_generated.h"
namespace y2017 {
namespace control_loops {
@@ -41,8 +42,10 @@
void Update(bool disabled);
// Fills out the ProfiledJointStatus structure with the current state.
- template <class StatusType>
- void PopulateTurretStatus(StatusType *status);
+ void PopulateTurretStatus(
+ TurretProfiledSubsystemStatus::Builder *status_builder,
+ flatbuffers::Offset<ColumnZeroingEstimator::State>
+ estimator_state_offset);
// Forces the current goal to the provided goal, bypassing the profiler.
void ForceGoal(double goal_velocity, double goal);
@@ -52,7 +55,7 @@
void set_turret_unprofiled_goal(double unprofiled_goal);
void set_unprofiled_goal(double goal_velocity, double unprofiled_goal);
// Limits our profiles to a max velocity and acceleration for proper motion.
- void AdjustProfile(const ::frc971::ProfileParameters &profile_parameters);
+ void AdjustProfile(const ::frc971::ProfileParameters *profile_parameters);
void AdjustProfile(double max_angular_velocity,
double max_angular_acceleration);
@@ -86,7 +89,7 @@
double IndexerStuckVoltage() const;
void PartialIndexerReset();
void PartialTurretReset();
- void PopulateIndexerStatus(IndexerStatus *status);
+ void PopulateIndexerStatus(IndexerStatus::Builder *status_builder);
void AddOffset(double indexer_offset_delta, double turret_offset_delta);
@@ -132,31 +135,6 @@
double turret_last_position_ = 0;
};
-template <typename StatusType>
-void ColumnProfiledSubsystem::PopulateTurretStatus(StatusType *status) {
- status->zeroed = zeroed();
- status->state = -1;
- // We don't know, so default to the bad case.
- status->estopped = true;
-
- status->position = X_hat(2, 0);
- status->velocity = X_hat(3, 0);
- status->goal_position = goal(2, 0);
- status->goal_velocity = goal(3, 0);
- status->unprofiled_goal_position = unprofiled_goal(2, 0);
- status->unprofiled_goal_velocity = unprofiled_goal(3, 0);
- status->voltage_error = X_hat(5, 0);
- status->calculated_velocity =
- (turret_position() - turret_last_position_) /
- ::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency);
-
- status->estimator_state = EstimatorState(0);
-
- Eigen::Matrix<double, 6, 1> error = controller().error();
- status->position_power = controller().controller().K(0, 0) * error(0, 0);
- status->velocity_power = controller().controller().K(0, 1) * error(1, 0);
-}
-
class Column {
public:
Column(::aos::EventLoop *event_loop);
@@ -179,15 +157,16 @@
static constexpr double kTurretMin = -0.1;
static constexpr double kTurretMax = M_PI / 2.0 + 0.1;
- void Iterate(::aos::monotonic_clock::time_point monotonic_now,
- const control_loops::IndexerGoal *unsafe_indexer_goal,
- const control_loops::TurretGoal *unsafe_turret_goal,
- const ColumnPosition *position,
- const vision::VisionStatus *vision_status,
- double *indexer_output, double *turret_output,
- IndexerStatus *indexer_status,
- TurretProfiledSubsystemStatus *turret_status,
- intake::Intake *intake);
+ std::pair<flatbuffers::Offset<IndexerStatus>,
+ flatbuffers::Offset<TurretProfiledSubsystemStatus>>
+ Iterate(::aos::monotonic_clock::time_point monotonic_now,
+ const IndexerGoalT *unsafe_indexer_goal,
+ const TurretGoal *unsafe_turret_goal, const ColumnPosition *position,
+ const vision::VisionStatus *vision_status, double *indexer_output,
+ double *turret_output, flatbuffers::FlatBufferBuilder *fbb,
+ // IndexerStatus *indexer_status,
+ // TurretProfiledSubsystemStatus *turret_status,
+ intake::Intake *intake);
void Reset();
diff --git a/y2017/control_loops/superstructure/column/column_zeroing.cc b/y2017/control_loops/superstructure/column/column_zeroing.cc
index 30500ca..7d5937a 100644
--- a/y2017/control_loops/superstructure/column/column_zeroing.cc
+++ b/y2017/control_loops/superstructure/column/column_zeroing.cc
@@ -31,15 +31,16 @@
}
void ColumnZeroingEstimator::UpdateEstimate(const ColumnPosition &position) {
- indexer_.UpdateEstimate(position.indexer);
- turret_.UpdateEstimate(position.turret);
+ indexer_.UpdateEstimate(*position.indexer());
+ turret_.UpdateEstimate(*position.turret());
if (indexer_.zeroed() && turret_.zeroed()) {
indexer_offset_ = indexer_.offset();
// Compute the current turret position.
- const double current_turret = indexer_offset_ + position.indexer.encoder +
- turret_.offset() + position.turret.encoder;
+ const double current_turret =
+ indexer_offset_ + position.indexer()->encoder() + turret_.offset() +
+ position.turret()->encoder();
// Now, we can compute the turret position which is closest to 0 radians
// (within +- M_PI).
@@ -47,16 +48,16 @@
::frc971::zeroing::Wrap(0.0, current_turret, M_PI * 2.0);
// Now, compute the actual turret offset.
- turret_offset_ = adjusted_turret - position.turret.encoder -
- (indexer_offset_ + position.indexer.encoder);
+ turret_offset_ = adjusted_turret - position.turret()->encoder() -
+ (indexer_offset_ + position.indexer()->encoder());
offset_ready_ = true;
// If we are close enough to 0, we are zeroed. Otherwise, we don't know
// which revolution we are on and need more info. We will always report the
// turret position as within +- M_PI from 0 with the provided offset.
- if (::std::abs(position.indexer.encoder + position.turret.encoder +
- indexer_offset_ + turret_offset_) <
- turret_zeroed_distance_) {
+ if (::std::abs(position.indexer()->encoder() +
+ position.turret()->encoder() + indexer_offset_ +
+ turret_offset_) < turret_zeroed_distance_) {
zeroed_ = true;
}
@@ -65,17 +66,23 @@
}
}
-ColumnZeroingEstimator::State ColumnZeroingEstimator::GetEstimatorState()
- const {
- State r;
- r.error = error();
- r.zeroed = zeroed();
- r.indexer = indexer_.GetEstimatorState();
- r.turret = turret_.GetEstimatorState();
- return r;
+flatbuffers::Offset<ColumnZeroingEstimator::State>
+ColumnZeroingEstimator::GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ flatbuffers::Offset<frc971::HallEffectAndPositionEstimatorState> indexer_offset =
+ indexer_.GetEstimatorState(fbb);
+ flatbuffers::Offset<frc971::HallEffectAndPositionEstimatorState> turret_offset =
+ turret_.GetEstimatorState(fbb);
+
+ State::Builder state_builder(*fbb);
+ state_builder.add_indexer(indexer_offset);
+ state_builder.add_turret(turret_offset);
+ state_builder.add_error(error());
+ state_builder.add_zeroed(zeroed());
+ return state_builder.Finish();
}
-} // column
-} // superstructure
+} // namespace column
+} // namespace superstructure
} // control_loops
} // y2017
diff --git a/y2017/control_loops/superstructure/column/column_zeroing.h b/y2017/control_loops/superstructure/column/column_zeroing.h
index f2ac94f..8c2cbf3 100644
--- a/y2017/control_loops/superstructure/column/column_zeroing.h
+++ b/y2017/control_loops/superstructure/column/column_zeroing.h
@@ -2,10 +2,10 @@
#define Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_COLUMN_H_
#include "frc971/constants.h"
-#include "frc971/control_loops/control_loops.q.h"
#include "frc971/zeroing/zeroing.h"
#include "y2017/constants.h"
-#include "y2017/control_loops/superstructure/superstructure.q.h"
+#include "y2017/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2017/control_loops/superstructure/superstructure_status_generated.h"
namespace y2017 {
namespace control_loops {
@@ -41,7 +41,8 @@
double turret_offset() const { return turret_offset_; }
// Returns information about our current state.
- State GetEstimatorState() const;
+ flatbuffers::Offset<State> GetEstimatorState(
+ flatbuffers::FlatBufferBuilder *fbb) const;
private:
// We are ensuring that two subsystems are zeroed, so here they are!
diff --git a/y2017/control_loops/superstructure/column/column_zeroing_test.cc b/y2017/control_loops/superstructure/column/column_zeroing_test.cc
index 3cc9c2e..dab5139 100644
--- a/y2017/control_loops/superstructure/column/column_zeroing_test.cc
+++ b/y2017/control_loops/superstructure/column/column_zeroing_test.cc
@@ -3,10 +3,13 @@
#include <random>
#include "aos/die.h"
+#include "aos/flatbuffers.h"
+#include "aos/json_to_flatbuffer.h"
#include "aos/testing/test_shm.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/team_number_test_environment.h"
#include "frc971/zeroing/zeroing.h"
+#include "glog/logging.h"
#include "gtest/gtest.h"
#include "y2017/constants.h"
#include "y2017/control_loops/superstructure/column/column_zeroing.h"
@@ -49,14 +52,28 @@
}
void MoveTo(double indexer, double turret) {
- ColumnPosition column_position;
+ flatbuffers::FlatBufferBuilder fbb;
indexer_sensor_.MoveTo(indexer);
turret_sensor_.MoveTo(turret - indexer);
- indexer_sensor_.GetSensorValues(&column_position.indexer);
- turret_sensor_.GetSensorValues(&column_position.turret);
+ HallEffectAndPosition::Builder indexer_builder(fbb);
+ flatbuffers::Offset<HallEffectAndPosition> indexer_offset =
+ indexer_sensor_.GetSensorValues(&indexer_builder);
- column_zeroing_estimator_.UpdateEstimate(column_position);
+ HallEffectAndPosition::Builder turret_builder(fbb);
+ flatbuffers::Offset<HallEffectAndPosition> turret_offset =
+ turret_sensor_.GetSensorValues(&turret_builder);
+
+ ColumnPosition::Builder column_position_builder(fbb);
+ column_position_builder.add_indexer(indexer_offset);
+ column_position_builder.add_turret(turret_offset);
+ fbb.Finish(column_position_builder.Finish());
+
+
+ aos::FlatbufferDetachedBuffer<ColumnPosition> column_position(fbb.Release());
+ LOG(INFO) << "Position: " << aos::FlatbufferToJson(column_position);
+
+ column_zeroing_estimator_.UpdateEstimate(column_position.message());
}
};
diff --git a/y2017/control_loops/superstructure/hood/BUILD b/y2017/control_loops/superstructure/hood/BUILD
index 9760eac..c3f6ef0 100644
--- a/y2017/control_loops/superstructure/hood/BUILD
+++ b/y2017/control_loops/superstructure/hood/BUILD
@@ -1,46 +1,48 @@
genrule(
- name = 'genrule_hood',
- cmd = '$(location //y2017/control_loops/python:hood) $(OUTS)',
- tools = [
- '//y2017/control_loops/python:hood',
- ],
- outs = [
- 'hood_plant.h',
- 'hood_plant.cc',
- 'hood_integral_plant.h',
- 'hood_integral_plant.cc',
- ],
+ name = "genrule_hood",
+ outs = [
+ "hood_plant.h",
+ "hood_plant.cc",
+ "hood_integral_plant.h",
+ "hood_integral_plant.cc",
+ ],
+ cmd = "$(location //y2017/control_loops/python:hood) $(OUTS)",
+ tools = [
+ "//y2017/control_loops/python:hood",
+ ],
)
cc_library(
- name = 'hood_plants',
- visibility = ['//visibility:public'],
- srcs = [
- 'hood_plant.cc',
- 'hood_integral_plant.cc',
- ],
- hdrs = [
- 'hood_plant.h',
- 'hood_integral_plant.h',
- ],
- deps = [
- '//frc971/control_loops:state_feedback_loop',
- ],
+ name = "hood_plants",
+ srcs = [
+ "hood_integral_plant.cc",
+ "hood_plant.cc",
+ ],
+ hdrs = [
+ "hood_integral_plant.h",
+ "hood_plant.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:state_feedback_loop",
+ ],
)
cc_library(
- name = 'hood',
- visibility = ['//visibility:public'],
- srcs = [
- 'hood.cc',
- ],
- hdrs = [
- 'hood.h',
- ],
- deps = [
- ':hood_plants',
- '//frc971/control_loops:profiled_subsystem',
- '//y2017/control_loops/superstructure:superstructure_queue',
- '//y2017:constants',
- ],
+ name = "hood",
+ srcs = [
+ "hood.cc",
+ ],
+ hdrs = [
+ "hood.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":hood_plants",
+ "//frc971/control_loops:profiled_subsystem",
+ "//y2017:constants",
+ "//y2017/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2017/control_loops/superstructure:superstructure_position_fbs",
+ "//y2017/control_loops/superstructure:superstructure_status_fbs",
+ ],
)
diff --git a/y2017/control_loops/superstructure/hood/hood.cc b/y2017/control_loops/superstructure/hood/hood.cc
index 8588a16..3dcd806 100644
--- a/y2017/control_loops/superstructure/hood/hood.cc
+++ b/y2017/control_loops/superstructure/hood/hood.cc
@@ -61,10 +61,12 @@
last_position_ = 0;
}
-void Hood::Iterate(const ::aos::monotonic_clock::time_point monotonic_now,
- const control_loops::HoodGoal *unsafe_goal,
- const ::frc971::IndexPosition *position, double *output,
- ::frc971::control_loops::IndexProfiledJointStatus *status) {
+flatbuffers::Offset<frc971::control_loops::IndexProfiledJointStatus>
+Hood::Iterate(const ::aos::monotonic_clock::time_point monotonic_now,
+ const double *unsafe_goal,
+ const frc971::ProfileParameters *unsafe_goal_profile_parameters,
+ const ::frc971::IndexPosition *position, double *output,
+ flatbuffers::FlatBufferBuilder *fbb) {
bool disable = output == nullptr;
profiled_subsystem_.Correct(*position);
@@ -151,8 +153,8 @@
// If we have a goal, go to it. Otherwise stay where we are.
if (unsafe_goal) {
- profiled_subsystem_.AdjustProfile(unsafe_goal->profile_params);
- profiled_subsystem_.set_unprofiled_goal(unsafe_goal->angle);
+ profiled_subsystem_.AdjustProfile(unsafe_goal_profile_parameters);
+ profiled_subsystem_.set_unprofiled_goal(*unsafe_goal);
}
// ESTOP if we hit the hard limits.
@@ -198,12 +200,14 @@
}
// Save debug/internal state.
+ frc971::control_loops::IndexProfiledJointStatus::Builder status_builder =
+ profiled_subsystem_.BuildStatus<
+ frc971::control_loops::IndexProfiledJointStatus::Builder>(fbb);
// TODO(austin): Save more.
- status->zeroed = profiled_subsystem_.zeroed();
+ status_builder.add_estopped((state_ == State::ESTOP));
+ status_builder.add_state(static_cast<int32_t>(state_));
- profiled_subsystem_.PopulateStatus(status);
- status->estopped = (state_ == State::ESTOP);
- status->state = static_cast<int32_t>(state_);
+ return status_builder.Finish();
}
} // namespace hood
diff --git a/y2017/control_loops/superstructure/hood/hood.h b/y2017/control_loops/superstructure/hood/hood.h
index 55d5344..7778152 100644
--- a/y2017/control_loops/superstructure/hood/hood.h
+++ b/y2017/control_loops/superstructure/hood/hood.h
@@ -2,8 +2,8 @@
#define Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_HOOD_HOOD_H_
#include "frc971/control_loops/profiled_subsystem.h"
-#include "y2017/control_loops/superstructure/superstructure.q.h"
#include "y2017/constants.h"
+#include "y2017/control_loops/superstructure/superstructure_goal_generated.h"
namespace y2017 {
namespace control_loops {
@@ -45,10 +45,12 @@
::std::chrono::milliseconds(15);
static constexpr double kNotMovingVoltage = 2.0;
- void Iterate(::aos::monotonic_clock::time_point monotonic_now,
- const control_loops::HoodGoal *unsafe_goal,
- const ::frc971::IndexPosition *position, double *output,
- ::frc971::control_loops::IndexProfiledJointStatus *status);
+ flatbuffers::Offset<frc971::control_loops::IndexProfiledJointStatus> Iterate(
+ ::aos::monotonic_clock::time_point monotonic_now,
+ const double *unsafe_goal,
+ const frc971::ProfileParameters *unsafe_goal_profile_parameters,
+ const ::frc971::IndexPosition *position, double *output,
+ flatbuffers::FlatBufferBuilder *fbb);
void Reset();
diff --git a/y2017/control_loops/superstructure/intake/BUILD b/y2017/control_loops/superstructure/intake/BUILD
index 164a63b..43f4927 100644
--- a/y2017/control_loops/superstructure/intake/BUILD
+++ b/y2017/control_loops/superstructure/intake/BUILD
@@ -1,46 +1,46 @@
genrule(
- name = 'genrule_intake',
- cmd = '$(location //y2017/control_loops/python:intake) $(OUTS)',
- tools = [
- '//y2017/control_loops/python:intake',
- ],
- outs = [
- 'intake_plant.h',
- 'intake_plant.cc',
- 'intake_integral_plant.h',
- 'intake_integral_plant.cc',
- ],
+ name = "genrule_intake",
+ outs = [
+ "intake_plant.h",
+ "intake_plant.cc",
+ "intake_integral_plant.h",
+ "intake_integral_plant.cc",
+ ],
+ cmd = "$(location //y2017/control_loops/python:intake) $(OUTS)",
+ tools = [
+ "//y2017/control_loops/python:intake",
+ ],
)
cc_library(
- name = 'intake_plants',
- visibility = ['//visibility:public'],
- srcs = [
- 'intake_plant.cc',
- 'intake_integral_plant.cc',
- ],
- hdrs = [
- 'intake_plant.h',
- 'intake_integral_plant.h',
- ],
- deps = [
- '//frc971/control_loops:state_feedback_loop',
- ],
+ name = "intake_plants",
+ srcs = [
+ "intake_integral_plant.cc",
+ "intake_plant.cc",
+ ],
+ hdrs = [
+ "intake_integral_plant.h",
+ "intake_plant.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:state_feedback_loop",
+ ],
)
cc_library(
- name = 'intake',
- visibility = ['//visibility:public'],
- srcs = [
- 'intake.cc',
- ],
- hdrs = [
- 'intake.h',
- ],
- deps = [
- ':intake_plants',
- '//frc971/control_loops:profiled_subsystem',
- '//y2017/control_loops/superstructure:superstructure_queue',
- '//y2017:constants',
- ],
+ name = "intake",
+ srcs = [
+ "intake.cc",
+ ],
+ hdrs = [
+ "intake.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":intake_plants",
+ "//frc971/control_loops:profiled_subsystem",
+ "//y2017:constants",
+ "//y2017/control_loops/superstructure:superstructure_goal_fbs",
+ ],
)
diff --git a/y2017/control_loops/superstructure/intake/intake.cc b/y2017/control_loops/superstructure/intake/intake.cc
index baa4693..ee631d7 100644
--- a/y2017/control_loops/superstructure/intake/intake.cc
+++ b/y2017/control_loops/superstructure/intake/intake.cc
@@ -29,10 +29,11 @@
disable_count_ = 0;
}
-void Intake::Iterate(
- const control_loops::IntakeGoal *unsafe_goal,
- const ::frc971::PotAndAbsolutePosition *position, double *output,
- ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus *status) {
+flatbuffers::Offset<
+ frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
+Intake::Iterate(const IntakeGoal *unsafe_goal,
+ const ::frc971::PotAndAbsolutePosition *position,
+ double *output, flatbuffers::FlatBufferBuilder *fbb) {
bool disable = output == nullptr;
profiled_subsystem_.Correct(*position);
@@ -84,9 +85,9 @@
}
if (unsafe_goal) {
- profiled_subsystem_.AdjustProfile(unsafe_goal->profile_params);
- profiled_subsystem_.set_unprofiled_goal(unsafe_goal->distance);
- if (unsafe_goal->disable_intake) {
+ profiled_subsystem_.AdjustProfile(unsafe_goal->profile_params());
+ profiled_subsystem_.set_unprofiled_goal(unsafe_goal->distance());
+ if (unsafe_goal->disable_intake()) {
if (disable_count_ > 100) {
disable = true;
}
@@ -133,13 +134,16 @@
*output = profiled_subsystem_.voltage();
}
- // Save debug/internal state.
- // TODO(austin): Save more.
- status->zeroed = profiled_subsystem_.zeroed();
+ frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus::Builder
+ status_builder = profiled_subsystem_.BuildStatus<
+ frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus::
+ Builder>(fbb);
- profiled_subsystem_.PopulateStatus(status);
- status->estopped = (state_ == State::ESTOP);
- status->state = static_cast<int32_t>(state_);
+ // Save debug/internal state.
+ status_builder.add_estopped((state_ == State::ESTOP));
+ status_builder.add_state(static_cast<int32_t>(state_));
+
+ return status_builder.Finish();
}
} // namespace intake
diff --git a/y2017/control_loops/superstructure/intake/intake.h b/y2017/control_loops/superstructure/intake/intake.h
index db1442a..cba3318 100644
--- a/y2017/control_loops/superstructure/intake/intake.h
+++ b/y2017/control_loops/superstructure/intake/intake.h
@@ -3,7 +3,7 @@
#include "frc971/control_loops/profiled_subsystem.h"
#include "y2017/constants.h"
-#include "y2017/control_loops/superstructure/superstructure.q.h"
+#include "y2017/control_loops/superstructure/superstructure_goal_generated.h"
namespace y2017 {
namespace control_loops {
@@ -32,10 +32,11 @@
static constexpr double kZeroingVoltage = 2.5;
static constexpr double kOperatingVoltage = 12.0;
- void Iterate(const control_loops::IntakeGoal *unsafe_goal,
- const ::frc971::PotAndAbsolutePosition *position, double *output,
- ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus
- *status);
+ flatbuffers::Offset<
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
+ Iterate(const IntakeGoal *unsafe_goal,
+ const ::frc971::PotAndAbsolutePosition *position, double *output,
+ flatbuffers::FlatBufferBuilder *fbb);
void Reset();
diff --git a/y2017/control_loops/superstructure/shooter/BUILD b/y2017/control_loops/superstructure/shooter/BUILD
index cb7fa54..d83e34b 100644
--- a/y2017/control_loops/superstructure/shooter/BUILD
+++ b/y2017/control_loops/superstructure/shooter/BUILD
@@ -1,49 +1,53 @@
-package(default_visibility = ['//visibility:public'])
+package(default_visibility = ["//visibility:public"])
genrule(
- name = 'genrule_shooter',
- cmd = '$(location //y2017/control_loops/python:shooter) $(OUTS)',
- tools = [
- '//y2017/control_loops/python:shooter',
- ],
- outs = [
- 'shooter_plant.h',
- 'shooter_plant.cc',
- 'shooter_integral_plant.h',
- 'shooter_integral_plant.cc',
- ],
+ name = "genrule_shooter",
+ outs = [
+ "shooter_plant.h",
+ "shooter_plant.cc",
+ "shooter_integral_plant.h",
+ "shooter_integral_plant.cc",
+ ],
+ cmd = "$(location //y2017/control_loops/python:shooter) $(OUTS)",
+ tools = [
+ "//y2017/control_loops/python:shooter",
+ ],
)
cc_library(
- name = 'shooter_plants',
- visibility = ['//visibility:public'],
- srcs = [
- 'shooter_plant.cc',
- 'shooter_integral_plant.cc',
- ],
- hdrs = [
- 'shooter_plant.h',
- 'shooter_integral_plant.h',
- ],
- deps = [
- '//frc971/control_loops:hybrid_state_feedback_loop',
- '//frc971/control_loops:state_feedback_loop',
- ],
+ name = "shooter_plants",
+ srcs = [
+ "shooter_integral_plant.cc",
+ "shooter_plant.cc",
+ ],
+ hdrs = [
+ "shooter_integral_plant.h",
+ "shooter_plant.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops:hybrid_state_feedback_loop",
+ "//frc971/control_loops:state_feedback_loop",
+ ],
)
cc_library(
- name = 'shooter',
- visibility = ['//visibility:public'],
- srcs = [
- 'shooter.cc',
- ],
- hdrs = [
- 'shooter.h',
- ],
- deps = [
- ':shooter_plants',
- '//aos/controls:control_loop',
- '//third_party/eigen',
- '//y2017/control_loops/superstructure:superstructure_queue',
- ],
+ name = "shooter",
+ srcs = [
+ "shooter.cc",
+ ],
+ hdrs = [
+ "shooter.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":shooter_plants",
+ "//aos/controls:control_loop",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//y2017/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2017/control_loops/superstructure:superstructure_position_fbs",
+ "//y2017/control_loops/superstructure:superstructure_status_fbs",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
)
diff --git a/y2017/control_loops/superstructure/shooter/shooter.cc b/y2017/control_loops/superstructure/shooter/shooter.cc
index 2b668ae..0ea4836 100644
--- a/y2017/control_loops/superstructure/shooter/shooter.cc
+++ b/y2017/control_loops/superstructure/shooter/shooter.cc
@@ -2,11 +2,7 @@
#include <chrono>
-#include "aos/controls/control_loops.q.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
-
-#include "y2017/control_loops/superstructure/shooter/shooter.h"
namespace y2017 {
namespace control_loops {
@@ -104,31 +100,35 @@
loop_->Update(disabled, dt);
}
-void ShooterController::SetStatus(ShooterStatus *status) {
- status->avg_angular_velocity = average_angular_velocity_;
+flatbuffers::Offset<ShooterStatus> ShooterController::BuildStatus(
+ flatbuffers::FlatBufferBuilder *fbb) {
+ ShooterStatus::Builder status_builder(*fbb);
+ status_builder.add_avg_angular_velocity(average_angular_velocity_);
- status->filtered_velocity = X_hat_current_(1, 0);
- status->angular_velocity = X_hat_current_(2, 0);
- status->ready = ready_;
+ status_builder.add_filtered_velocity(X_hat_current_(1, 0));
+ status_builder.add_angular_velocity(X_hat_current_(2, 0));
+ status_builder.add_ready(ready_);
- status->voltage_error = X_hat_current_(3, 0);
- status->position_error = position_error_;
- status->instantaneous_velocity = dt_velocity_;
- status->fixed_instantaneous_velocity = fixed_dt_velocity_;
+ status_builder.add_voltage_error(X_hat_current_(3, 0));
+ status_builder.add_position_error(position_error_);
+ status_builder.add_instantaneous_velocity(dt_velocity_);
+ status_builder.add_fixed_instantaneous_velocity(fixed_dt_velocity_);
+
+ return status_builder.Finish();
}
void Shooter::Reset() { wheel_.Reset(); }
-void Shooter::Iterate(const control_loops::ShooterGoal *goal,
- const double *position,
- ::aos::monotonic_clock::time_point position_time,
- double *output, control_loops::ShooterStatus *status) {
+flatbuffers::Offset<ShooterStatus> Shooter::Iterate(
+ const ShooterGoalT *goal, const double position,
+ ::aos::monotonic_clock::time_point position_time, double *output,
+ flatbuffers::FlatBufferBuilder *fbb) {
if (goal) {
// Update position/goal for our wheel.
wheel_.set_goal(goal->angular_velocity);
}
- wheel_.set_position(*position);
+ wheel_.set_position(position);
chrono::nanoseconds dt = ::aos::controls::kLoopFrequency;
if (last_time_ != ::aos::monotonic_clock::min_time) {
@@ -138,20 +138,22 @@
wheel_.Update(output == nullptr, dt);
- wheel_.SetStatus(status);
+ flatbuffers::Offset<ShooterStatus> status_offset = wheel_.BuildStatus(fbb);
- if (last_ready_ && !status->ready) {
+ if (last_ready_ && !wheel_.ready()) {
min_ = wheel_.dt_velocity();
- } else if (!status->ready) {
+ } else if (!wheel_.ready()) {
min_ = ::std::min(min_, wheel_.dt_velocity());
- } else if (!last_ready_ && status->ready) {
+ } else if (!last_ready_ && wheel_.ready()) {
AOS_LOG(INFO, "Shot min was [%f]\n", min_);
}
if (output) {
*output = wheel_.voltage();
}
- last_ready_ = status->ready;
+ last_ready_ = wheel_.ready();
+
+ return status_offset;
}
} // namespace shooter
diff --git a/y2017/control_loops/superstructure/shooter/shooter.h b/y2017/control_loops/superstructure/shooter/shooter.h
index e8f7789..19fc7dd 100644
--- a/y2017/control_loops/superstructure/shooter/shooter.h
+++ b/y2017/control_loops/superstructure/shooter/shooter.h
@@ -7,10 +7,11 @@
#include "aos/controls/control_loop.h"
#include "aos/time/time.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "third_party/eigen/Eigen/Dense"
+#include "Eigen/Dense"
#include "y2017/control_loops/superstructure/shooter/shooter_integral_plant.h"
-#include "y2017/control_loops/superstructure/superstructure.q.h"
+#include "y2017/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2017/control_loops/superstructure/superstructure_status_generated.h"
namespace y2017 {
namespace control_loops {
@@ -27,7 +28,8 @@
void set_position(double current_position);
// Populates the status structure.
- void SetStatus(control_loops::ShooterStatus *status);
+ flatbuffers::Offset<ShooterStatus> BuildStatus(
+ flatbuffers::FlatBufferBuilder *fbb);
// Returns the control loop calculated voltage.
double voltage() const;
@@ -46,6 +48,8 @@
// Resets the kalman filter and any other internal state.
void Reset();
+ bool ready() { return ready_; }
+
private:
// The current sensor measurement.
Eigen::Matrix<double, 1, 1> Y_;
@@ -86,9 +90,10 @@
// Iterates the shooter control loop one cycle. position and status must
// never be nullptr. goal can be nullptr if no goal exists, and output should
// be nullptr if disabled.
- void Iterate(const control_loops::ShooterGoal *goal, const double *position,
- ::aos::monotonic_clock::time_point position_time, double *output,
- control_loops::ShooterStatus *status);
+ flatbuffers::Offset<ShooterStatus> Iterate(
+ const ShooterGoalT *goal, const double position,
+ ::aos::monotonic_clock::time_point position_time, double *output,
+ flatbuffers::FlatBufferBuilder *fbb);
// Sets the shooter up to reset the kalman filter next time Iterate is called.
void Reset();
diff --git a/y2017/control_loops/superstructure/superstructure.cc b/y2017/control_loops/superstructure/superstructure.cc
index 3ad1747..29886e2 100644
--- a/y2017/control_loops/superstructure/superstructure.cc
+++ b/y2017/control_loops/superstructure/superstructure.cc
@@ -1,14 +1,13 @@
#include "y2017/control_loops/superstructure/superstructure.h"
-#include "aos/controls/control_loops.q.h"
#include "aos/logging/logging.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2017/constants.h"
#include "y2017/control_loops/superstructure/column/column.h"
#include "y2017/control_loops/superstructure/hood/hood.h"
#include "y2017/control_loops/superstructure/intake/intake.h"
#include "y2017/control_loops/superstructure/shooter/shooter.h"
-#include "y2017/vision/vision.q.h"
+#include "y2017/vision/vision_generated.h"
namespace y2017 {
namespace control_loops {
@@ -24,19 +23,18 @@
Superstructure::Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name)
- : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(event_loop,
- name),
+ : aos::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
+ name),
vision_status_fetcher_(
- event_loop->MakeFetcher<::y2017::vision::VisionStatus>(
- ".y2017.vision.vision_status")),
+ event_loop->MakeFetcher<::y2017::vision::VisionStatus>("/vision")),
drivetrain_status_fetcher_(
- event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
- ".frc971.control_loops.drivetrain_queue.status")),
+ event_loop->MakeFetcher<::frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")),
column_(event_loop) {
shot_interpolation_table_ =
::frc971::shooter_interpolation::InterpolationTable<ShotParams>({
- // { distance_to_target, { shot_angle, shot_power, indexer_velocity }},
+ // { distance_to_target, { shot_angle, shot_power, indexer_velocity
+ // }},
{1.21, {0.29, 301.0, -1.0 * M_PI}}, // table entry
{1.55, {0.305, 316.0, -1.1 * M_PI}}, // table entry
{1.82, {0.33, 325.0, -1.3 * M_PI}}, // table entry
@@ -51,10 +49,11 @@
}
void Superstructure::RunIteration(
- const control_loops::SuperstructureQueue::Goal *unsafe_goal,
- const control_loops::SuperstructureQueue::Position *position,
- control_loops::SuperstructureQueue::Output *output,
- control_loops::SuperstructureQueue::Status *status) {
+ const Goal *unsafe_goal,
+ const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) {
+ OutputT output_struct;
const ::aos::monotonic_clock::time_point monotonic_now =
event_loop()->monotonic_now();
if (WasReset()) {
@@ -71,41 +70,43 @@
}
// Create a copy of the goals so that we can modify them.
- HoodGoal hood_goal;
- ShooterGoal shooter_goal;
- IndexerGoal indexer_goal;
+ double hood_goal_angle = 0.0;
+ ShooterGoalT shooter_goal;
+ IndexerGoalT indexer_goal;
bool in_range = true;
+ double vision_distance = 0.0;
if (unsafe_goal != nullptr) {
- hood_goal = unsafe_goal->hood;
- shooter_goal = unsafe_goal->shooter;
- indexer_goal = unsafe_goal->indexer;
+ hood_goal_angle = unsafe_goal->hood()->angle();
+ shooter_goal.angular_velocity = unsafe_goal->shooter()->angular_velocity();
+ indexer_goal.angular_velocity = unsafe_goal->indexer()->angular_velocity();
+ indexer_goal.voltage_rollers = unsafe_goal->indexer()->voltage_rollers();
- if (!unsafe_goal->use_vision_for_shots) {
+ if (!unsafe_goal->use_vision_for_shots()) {
distance_average_.Reset();
}
distance_average_.Tick(monotonic_now, vision_status);
- status->vision_distance = distance_average_.Get();
+ vision_distance = distance_average_.Get();
// If we are moving too fast, disable shooting and clear the accumulator.
double robot_velocity = 0.0;
drivetrain_status_fetcher_.Fetch();
if (drivetrain_status_fetcher_.get()) {
- robot_velocity = drivetrain_status_fetcher_->robot_speed;
+ robot_velocity = drivetrain_status_fetcher_->robot_speed();
}
if (::std::abs(robot_velocity) > 0.2) {
- if (unsafe_goal->use_vision_for_shots) {
+ if (unsafe_goal->use_vision_for_shots()) {
AOS_LOG(INFO, "Moving too fast, resetting\n");
}
distance_average_.Reset();
}
if (distance_average_.Valid()) {
- if (unsafe_goal->use_vision_for_shots) {
+ if (unsafe_goal->use_vision_for_shots()) {
ShotParams shot_params;
if (shot_interpolation_table_.GetInRange(
distance_average_.Get(), &shot_params)) {
- hood_goal.angle = shot_params.angle;
+ hood_goal_angle = shot_params.angle;
shooter_goal.angular_velocity = shot_params.power;
if (indexer_goal.angular_velocity != 0.0) {
indexer_goal.angular_velocity = shot_params.indexer_velocity;
@@ -116,25 +117,30 @@
}
AOS_LOG(
DEBUG, "VisionDistance %f, hood %f shooter %f, indexer %f * M_PI\n",
- status->vision_distance, hood_goal.angle,
+ vision_distance, hood_goal_angle,
shooter_goal.angular_velocity, indexer_goal.angular_velocity / M_PI);
} else {
- AOS_LOG(DEBUG, "VisionNotValid %f\n", status->vision_distance);
- if (unsafe_goal->use_vision_for_shots) {
+ AOS_LOG(DEBUG, "VisionNotValid %f\n", vision_distance);
+ if (unsafe_goal->use_vision_for_shots()) {
in_range = false;
indexer_goal.angular_velocity = 0.0;
}
}
}
- hood_.Iterate(monotonic_now, unsafe_goal != nullptr ? &hood_goal : nullptr,
- &(position->hood),
- output != nullptr ? &(output->voltage_hood) : nullptr,
- &(status->hood));
- shooter_.Iterate(unsafe_goal != nullptr ? &shooter_goal : nullptr,
- &(position->theta_shooter), position->sent_time,
- output != nullptr ? &(output->voltage_shooter) : nullptr,
- &(status->shooter));
+ flatbuffers::Offset<frc971::control_loops::IndexProfiledJointStatus>
+ hood_offset = hood_.Iterate(
+ monotonic_now, unsafe_goal != nullptr ? &hood_goal_angle : nullptr,
+ unsafe_goal != nullptr ? unsafe_goal->hood()->profile_params()
+ : nullptr,
+ position->hood(),
+ output != nullptr ? &(output_struct.voltage_hood) : nullptr,
+ status->fbb());
+ flatbuffers::Offset<ShooterStatus> shooter_offset = shooter_.Iterate(
+ unsafe_goal != nullptr ? &shooter_goal : nullptr,
+ position->theta_shooter(), position_context().monotonic_sent_time,
+ output != nullptr ? &(output_struct.voltage_shooter) : nullptr,
+ status->fbb());
// Implement collision avoidance by passing down a freeze or range restricting
// signal to the column and intake objects.
@@ -145,8 +151,8 @@
// The turret is in a position (or wants to be in a position) where we
// need the intake out. Push it out.
const bool column_goal_not_safe =
- unsafe_goal->turret.angle > column::Column::kTurretMax ||
- unsafe_goal->turret.angle < column::Column::kTurretMin;
+ unsafe_goal->turret()->angle() > column::Column::kTurretMax ||
+ unsafe_goal->turret()->angle() < column::Column::kTurretMin;
const bool column_position_not_safe =
column_.turret_position() > column::Column::kTurretMax ||
column_.turret_position() < column::Column::kTurretMin;
@@ -178,55 +184,87 @@
AOS_LOG(ERROR, "Collisions ignored\n");
}
- intake_.Iterate(unsafe_goal != nullptr ? &(unsafe_goal->intake) : nullptr,
- &(position->intake),
- output != nullptr ? &(output->voltage_intake) : nullptr,
- &(status->intake));
+ flatbuffers::Offset<
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
+ intake_offset = intake_.Iterate(
+ unsafe_goal != nullptr ? unsafe_goal->intake() : nullptr,
+ position->intake(),
+ output != nullptr ? &(output_struct.voltage_intake) : nullptr,
+ status->fbb());
- column_.Iterate(monotonic_now,
- unsafe_goal != nullptr ? &indexer_goal : nullptr,
- unsafe_goal != nullptr ? &(unsafe_goal->turret) : nullptr,
- &(position->column), vision_status,
- output != nullptr ? &(output->voltage_indexer) : nullptr,
- output != nullptr ? &(output->voltage_turret) : nullptr,
- &(status->indexer), &(status->turret), &intake_);
+ std::pair<flatbuffers::Offset<IndexerStatus>,
+ flatbuffers::Offset<TurretProfiledSubsystemStatus>>
+ indexer_and_turret_offsets = column_.Iterate(
+ monotonic_now, unsafe_goal != nullptr ? &indexer_goal : nullptr,
+ unsafe_goal != nullptr ? unsafe_goal->turret() : nullptr,
+ position->column(), vision_status,
+ output != nullptr ? &(output_struct.voltage_indexer) : nullptr,
+ output != nullptr ? &(output_struct.voltage_turret) : nullptr,
+ status->fbb(), &intake_);
- status->estopped =
- status->intake.estopped | status->hood.estopped | status->turret.estopped;
+ const frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus
+ *temp_intake_status = GetTemporaryPointer(*status->fbb(), intake_offset);
+ const frc971::control_loops::IndexProfiledJointStatus *temp_hood_status =
+ GetTemporaryPointer(*status->fbb(), hood_offset);
+ const TurretProfiledSubsystemStatus *temp_turret_status =
+ GetTemporaryPointer(*status->fbb(), indexer_and_turret_offsets.second);
- status->zeroed =
- status->intake.zeroed && status->hood.zeroed && status->turret.zeroed;
+ const bool estopped = temp_intake_status->estopped() ||
+ temp_hood_status->estopped() ||
+ temp_turret_status->estopped();
+ const bool zeroed = temp_intake_status->zeroed() &&
+ temp_hood_status->zeroed() &&
+ temp_turret_status->zeroed();
+ const bool turret_vision_tracking = temp_turret_status->vision_tracking();
+
+ Status::Builder status_builder = status->MakeBuilder<Status>();
+ status_builder.add_intake(intake_offset);
+ status_builder.add_hood(hood_offset);
+ status_builder.add_shooter(shooter_offset);
+ status_builder.add_turret(indexer_and_turret_offsets.second);
+ status_builder.add_indexer(indexer_and_turret_offsets.first);
+
+ status_builder.add_estopped(estopped);
+ status_builder.add_zeroed(zeroed);
+
+ status_builder.add_vision_distance(vision_distance);
if (output && unsafe_goal) {
- output->gear_servo =
- ::std::min(1.0, ::std::max(0.0, unsafe_goal->intake.gear_servo));
+ output_struct.gear_servo =
+ ::std::min(1.0, ::std::max(0.0, unsafe_goal->intake()->gear_servo()));
- output->voltage_intake_rollers =
+ output_struct.voltage_intake_rollers =
::std::max(-kMaxIntakeRollerVoltage,
- ::std::min(unsafe_goal->intake.voltage_rollers,
+ ::std::min(unsafe_goal->intake()->voltage_rollers(),
kMaxIntakeRollerVoltage));
- output->voltage_indexer_rollers =
+ output_struct.voltage_indexer_rollers =
::std::max(-kMaxIndexerRollerVoltage,
- ::std::min(unsafe_goal->indexer.voltage_rollers,
+ ::std::min(unsafe_goal->indexer()->voltage_rollers(),
kMaxIndexerRollerVoltage));
// Set the lights on or off
- output->lights_on = unsafe_goal->lights_on;
+ output_struct.lights_on = unsafe_goal->lights_on();
- if (status->estopped) {
- output->red_light_on = true;
- output->green_light_on = false;
- output->blue_light_on = false;
- } else if (!status->zeroed) {
- output->red_light_on = false;
- output->green_light_on = false;
- output->blue_light_on = true;
- } else if (status->turret.vision_tracking && in_range) {
- output->red_light_on = false;
- output->green_light_on = true;
- output->blue_light_on = false;
+ if (estopped) {
+ output_struct.red_light_on = true;
+ output_struct.green_light_on = false;
+ output_struct.blue_light_on = false;
+ } else if (!zeroed) {
+ output_struct.red_light_on = false;
+ output_struct.green_light_on = false;
+ output_struct.blue_light_on = true;
+ } else if (turret_vision_tracking && in_range) {
+ output_struct.red_light_on = false;
+ output_struct.green_light_on = true;
+ output_struct.blue_light_on = false;
}
}
+
+ if (output) {
+ output->Send(Output::Pack(*output->fbb(), &output_struct));
+ }
+
+ status->Send(status_builder.Finish());
}
} // namespace superstructure
diff --git a/y2017/control_loops/superstructure/superstructure.h b/y2017/control_loops/superstructure/superstructure.h
index 660b861..a4c824e 100644
--- a/y2017/control_loops/superstructure/superstructure.h
+++ b/y2017/control_loops/superstructure/superstructure.h
@@ -4,13 +4,16 @@
#include <memory>
#include "aos/controls/control_loop.h"
-#include "aos/events/event-loop.h"
+#include "aos/events/event_loop.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "y2017/control_loops/superstructure/column/column.h"
#include "y2017/control_loops/superstructure/hood/hood.h"
#include "y2017/control_loops/superstructure/intake/intake.h"
#include "y2017/control_loops/superstructure/shooter/shooter.h"
-#include "y2017/control_loops/superstructure/superstructure.q.h"
+#include "y2017/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2017/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2017/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2017/control_loops/superstructure/superstructure_status_generated.h"
#include "y2017/control_loops/superstructure/vision_distance_average.h"
namespace y2017 {
@@ -18,11 +21,10 @@
namespace superstructure {
class Superstructure
- : public ::aos::controls::ControlLoop<control_loops::SuperstructureQueue> {
+ : public ::aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
- explicit Superstructure(
- ::aos::EventLoop *event_loop,
- const ::std::string &name = ".y2017.control_loops.superstructure_queue");
+ explicit Superstructure(::aos::EventLoop *event_loop,
+ const ::std::string &name = "/superstructure");
const hood::Hood &hood() const { return hood_; }
const intake::Intake &intake() const { return intake_; }
@@ -35,15 +37,13 @@
}
protected:
- virtual void RunIteration(
- const control_loops::SuperstructureQueue::Goal *unsafe_goal,
- const control_loops::SuperstructureQueue::Position *position,
- control_loops::SuperstructureQueue::Output *output,
- control_loops::SuperstructureQueue::Status *status) override;
+ virtual void RunIteration(const Goal *unsafe_goal, const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) override;
private:
::aos::Fetcher<::y2017::vision::VisionStatus> vision_status_fetcher_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
hood::Hood hood_;
diff --git a/y2017/control_loops/superstructure/superstructure.q b/y2017/control_loops/superstructure/superstructure.q
deleted file mode 100644
index 6ea2fa2..0000000
--- a/y2017/control_loops/superstructure/superstructure.q
+++ /dev/null
@@ -1,258 +0,0 @@
-package y2017.control_loops;
-
-import "aos/controls/control_loops.q";
-import "frc971/control_loops/profiled_subsystem.q";
-// TODO(austin): Add this back in when the queue compiler supports diamond
-// inheritance.
-//import "frc971/control_loops/control_loops.q";
-
-struct IntakeGoal {
- // Zero for the intake is when the front tube is tangent with the front of the
- // frame. Positive is out.
-
- // Goal distance of the intake.
- double distance;
-
- // Caps on velocity/acceleration for profiling. 0 for the default.
- .frc971.ProfileParameters profile_params;
-
- // Voltage to send to the rollers. Positive is sucking in.
- double voltage_rollers;
-
- // If true, disable the intake so we can hang.
- bool disable_intake;
-
- // The gear servo value.
- double gear_servo;
-};
-
-struct IndexerGoal {
- // Indexer angular velocity goals in radians/second.
- double angular_velocity;
-
- // Roller voltage. Positive is sucking in.
- double voltage_rollers;
-};
-
-struct TurretGoal {
- // An angle of zero means the turrent faces toward the front of the
- // robot where the intake is located. The angle increases when the turret
- // turns clockwise (towards right from the front), and decreases when
- // the turrent turns counter-clockwise (towards left from the front).
- // These are from a top view above the robot.
- double angle;
-
- // If true, ignore the angle and track using vision. If we don't see
- // anything, we'll use the turret goal above.
- bool track;
-
- // Caps on velocity/acceleration for profiling. 0 for the default.
- .frc971.ProfileParameters profile_params;
-};
-
-struct HoodGoal {
- // Angle the hood is currently at. An angle of zero is at the lower hard
- // stop, angle increases as hood rises.
- double angle;
-
- // Caps on velocity/acceleration for profiling. 0 for the default.
- .frc971.ProfileParameters profile_params;
-};
-
-struct ShooterGoal {
- // Angular velocity goals in radians/second. Positive is shooting out of the
- // robot.
- double angular_velocity;
-};
-
-struct IndexerStatus {
- // The current average velocity in radians/second. Positive is moving balls up
- // towards the shooter. This is the angular velocity of the inner piece.
- double avg_angular_velocity;
-
- // The current instantaneous filtered velocity in radians/second.
- double angular_velocity;
-
- // True if the indexer is ready. It is better to compare the velocities
- // directly so there isn't confusion on if the goal is up to date.
- bool ready;
-
- // True if the indexer is stuck.
- bool stuck;
- float stuck_voltage;
-
- // The state of the indexer state machine.
- int32_t state;
-
- // The estimated voltage error from the kalman filter in volts.
- double voltage_error;
- // The estimated voltage error from the stuck indexer kalman filter.
- double stuck_voltage_error;
-
- // The current velocity measured as delta x / delta t in radians/sec.
- double instantaneous_velocity;
-
- // The error between our measurement and expected measurement in radians.
- double position_error;
-};
-
-struct ShooterStatus {
- // The current average velocity in radians/second.
- double avg_angular_velocity;
-
- // The current instantaneous filtered velocity in radians/second.
- double angular_velocity;
-
- // True if the shooter is ready. It is better to compare the velocities
- // directly so there isn't confusion on if the goal is up to date.
- bool ready;
-
- // The estimated voltage error from the kalman filter in volts.
- double voltage_error;
-
- // The current velocity measured as delta x / delta t in radians/sec.
- double instantaneous_velocity;
- double filtered_velocity;
- double fixed_instantaneous_velocity;
-
- // The error between our measurement and expected measurement in radians.
- double position_error;
-};
-
-struct ColumnPosition {
- // Indexer angle in radians relative to the base. Positive is according to
- // the right hand rule around +z.
- .frc971.HallEffectAndPosition indexer;
- // Turret angle in radians relative to the indexer. Positive is the same as
- // the indexer.
- .frc971.HallEffectAndPosition turret;
-};
-
-struct ColumnEstimatorState {
- bool error;
- bool zeroed;
- .frc971.HallEffectAndPositionEstimatorState indexer;
- .frc971.HallEffectAndPositionEstimatorState turret;
-};
-
-struct TurretProfiledSubsystemStatus {
- // Is the subsystem zeroed?
- bool zeroed;
-
- // The state of the subsystem, if applicable. -1 otherwise.
- int32_t state;
-
- // If true, we have aborted.
- bool estopped;
-
- // Position of the joint.
- float position;
- // Velocity of the joint in units/second.
- float velocity;
- // Profiled goal position of the joint.
- float goal_position;
- // Profiled goal velocity of the joint in units/second.
- float goal_velocity;
- // Unprofiled goal position from absoulte zero of the joint.
- float unprofiled_goal_position;
- // Unprofiled goal velocity of the joint in units/second.
- float unprofiled_goal_velocity;
-
- // The estimated voltage error.
- float voltage_error;
-
- // The calculated velocity with delta x/delta t
- float calculated_velocity;
-
- // Components of the control loop output
- float position_power;
- float velocity_power;
- float feedforwards_power;
-
- // State of the estimator.
- ColumnEstimatorState estimator_state;
-
- double raw_vision_angle;
- double vision_angle;
- bool vision_tracking;
-
- double turret_encoder_angle;
-};
-
-// Published on ".y2017.control_loops.superstructure_queue"
-queue_group SuperstructureQueue {
- implements aos.control_loops.ControlLoop;
-
- message Goal {
- IntakeGoal intake;
- IndexerGoal indexer;
- TurretGoal turret;
- HoodGoal hood;
- ShooterGoal shooter;
- bool lights_on;
- bool use_vision_for_shots;
- };
-
- message Status {
- // Are all the subsystems zeroed?
- bool zeroed;
-
- // If true, we have aborted. This is the or of all subsystem estops.
- bool estopped;
-
- // Each subsystems status.
- .frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus intake;
- .frc971.control_loops.IndexProfiledJointStatus hood;
- ShooterStatus shooter;
-
- TurretProfiledSubsystemStatus turret;
- IndexerStatus indexer;
-
- float vision_distance;
- };
-
- message Position {
- // Position of the intake, zero when the intake is in, positive when it is
- // out.
- .frc971.PotAndAbsolutePosition intake;
-
- // The position of the column.
- ColumnPosition column;
-
- // The sensor readings for the hood. The units and sign are defined the
- // same as what's in the Goal message.
- .frc971.IndexPosition hood;
-
- // Shooter wheel angle in radians.
- double theta_shooter;
- };
-
- message Output {
- // Voltages for some of the subsystems.
- double voltage_intake;
- double voltage_indexer;
- double voltage_shooter;
-
- // Rollers on the intake.
- double voltage_intake_rollers;
- // Roller on the indexer
- double voltage_indexer_rollers;
-
- double voltage_turret;
- double voltage_hood;
-
- double gear_servo;
-
- // If true, the lights are on.
- bool lights_on;
-
- bool red_light_on;
- bool green_light_on;
- bool blue_light_on;
- };
-
- queue Goal goal;
- queue Position position;
- queue Output output;
- queue Status status;
-};
diff --git a/y2017/control_loops/superstructure/superstructure_goal.fbs b/y2017/control_loops/superstructure/superstructure_goal.fbs
new file mode 100644
index 0000000..6681262
--- /dev/null
+++ b/y2017/control_loops/superstructure/superstructure_goal.fbs
@@ -0,0 +1,74 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2017.control_loops.superstructure;
+
+table IntakeGoal {
+ // Zero for the intake is when the front tube is tangent with the front of the
+ // frame. Positive is out.
+
+ // Goal distance of the intake.
+ distance:double;
+
+ // Caps on velocity/acceleration for profiling. 0 for the default.
+ profile_params:frc971.ProfileParameters;
+
+ // Voltage to send to the rollers. Positive is sucking in.
+ voltage_rollers:double;
+
+ // If true, disable the intake so we can hang.
+ disable_intake:bool;
+
+ // The gear servo value.
+ gear_servo:double;
+}
+
+table IndexerGoal {
+ // Indexer angular velocity goals in radians/second.
+ angular_velocity:double;
+
+ // Roller voltage. Positive is sucking in.
+ voltage_rollers:double;
+}
+
+table TurretGoal {
+ // An angle of zero means the turrent faces toward the front of the
+ // robot where the intake is located. The angle increases when the turret
+ // turns clockwise (towards right from the front), and decreases when
+ // the turrent turns counter-clockwise (towards left from the front).
+ // These are from a top view above the robot.
+ angle:double;
+
+ // If true, ignore the angle and track using vision. If we don't see
+ // anything, we'll use the turret goal above.
+ track:bool;
+
+ // Caps on velocity/acceleration for profiling. 0 for the default.
+ profile_params:frc971.ProfileParameters;
+}
+
+table HoodGoal {
+ // Angle the hood is currently at. An angle of zero is at the lower hard
+ // stop, angle increases as hood rises.
+ angle:double;
+
+ // Caps on velocity/acceleration for profiling. 0 for the default.
+ profile_params:frc971.ProfileParameters;
+}
+
+table ShooterGoal {
+ // Angular velocity goals in radians/second. Positive is shooting out of the
+ // robot.
+ angular_velocity:double;
+}
+
+table Goal {
+ intake:IntakeGoal;
+ indexer:IndexerGoal;
+ turret:TurretGoal;
+ hood:HoodGoal;
+ shooter:ShooterGoal;
+ lights_on:bool;
+ use_vision_for_shots:bool;
+}
+
+root_type Goal;
diff --git a/y2017/control_loops/superstructure/superstructure_lib_test.cc b/y2017/control_loops/superstructure/superstructure_lib_test.cc
index 1e763b8..5298687 100644
--- a/y2017/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2017/control_loops/superstructure/superstructure_lib_test.cc
@@ -6,7 +6,6 @@
#include <memory>
#include "aos/controls/control_loop_test.h"
-#include "aos/queue.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
@@ -121,14 +120,11 @@
: event_loop_(event_loop),
dt_(dt),
superstructure_position_sender_(
- event_loop_->MakeSender<SuperstructureQueue::Position>(
- ".y2017.control_loops.superstructure_queue.position")),
+ event_loop_->MakeSender<Position>("/superstructure")),
superstructure_status_fetcher_(
- event_loop_->MakeFetcher<SuperstructureQueue::Status>(
- ".y2017.control_loops.superstructure_queue.status")),
+ event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_output_fetcher_(
- event_loop_->MakeFetcher<SuperstructureQueue::Output>(
- ".y2017.control_loops.superstructure_queue.output")),
+ event_loop_->MakeFetcher<Output>("/superstructure")),
hood_plant_(new HoodPlant(
::y2017::control_loops::superstructure::hood::MakeHoodPlant())),
hood_encoder_(constants::Values::kHoodEncoderIndexDifference),
@@ -216,17 +212,42 @@
// Sends a queue message with the position of the superstructure.
void SendPositionMessage() {
- ::aos::Sender<SuperstructureQueue::Position>::Message position =
- superstructure_position_sender_.MakeMessage();
+ ::aos::Sender<Position>::Builder builder =
+ superstructure_position_sender_.MakeBuilder();
- hood_encoder_.GetSensorValues(&position->hood);
- intake_pot_encoder_.GetSensorValues(&position->intake);
- position->theta_shooter = shooter_plant_->Y(0, 0);
+ frc971::IndexPosition::Builder hood_builder =
+ builder.MakeBuilder<frc971::IndexPosition>();
+ flatbuffers::Offset<frc971::IndexPosition> hood_offset =
+ hood_encoder_.GetSensorValues(&hood_builder);
- turret_encoder_.GetSensorValues(&position->column.turret);
- indexer_encoder_.GetSensorValues(&position->column.indexer);
+ frc971::PotAndAbsolutePosition::Builder intake_builder =
+ builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> intake_offset =
+ intake_pot_encoder_.GetSensorValues(&intake_builder);
- position.Send();
+ frc971::HallEffectAndPosition::Builder turret_builder =
+ builder.MakeBuilder<frc971::HallEffectAndPosition>();
+ flatbuffers::Offset<frc971::HallEffectAndPosition> turret_offset =
+ turret_encoder_.GetSensorValues(&turret_builder);
+
+ frc971::HallEffectAndPosition::Builder indexer_builder =
+ builder.MakeBuilder<frc971::HallEffectAndPosition>();
+ flatbuffers::Offset<frc971::HallEffectAndPosition> indexer_offset =
+ indexer_encoder_.GetSensorValues(&indexer_builder);
+
+ ColumnPosition::Builder column_builder =
+ builder.MakeBuilder<ColumnPosition>();
+ column_builder.add_indexer(indexer_offset);
+ column_builder.add_turret(turret_offset);
+ flatbuffers::Offset<ColumnPosition> column_offset = column_builder.Finish();
+
+ Position::Builder position_builder = builder.MakeBuilder<Position>();
+ position_builder.add_theta_shooter(shooter_plant_->Y(0, 0));
+ position_builder.add_column(column_offset);
+ position_builder.add_intake(intake_offset);
+ position_builder.add_hood(hood_offset);
+
+ builder.Send(position_builder.Finish());
}
double hood_position() const { return hood_plant_->X(0, 0); }
@@ -282,61 +303,61 @@
const double voltage_check_hood =
(static_cast<hood::Hood::State>(
- superstructure_status_fetcher_->hood.state) ==
+ superstructure_status_fetcher_->hood()->state()) ==
hood::Hood::State::RUNNING)
? superstructure::hood::Hood::kOperatingVoltage
: superstructure::hood::Hood::kZeroingVoltage;
const double voltage_check_indexer =
(static_cast<column::Column::State>(
- superstructure_status_fetcher_->turret.state) ==
+ superstructure_status_fetcher_->turret()->state()) ==
column::Column::State::RUNNING)
? superstructure::column::Column::kOperatingVoltage
: superstructure::column::Column::kZeroingVoltage;
const double voltage_check_turret =
(static_cast<column::Column::State>(
- superstructure_status_fetcher_->turret.state) ==
+ superstructure_status_fetcher_->turret()->state()) ==
column::Column::State::RUNNING)
? superstructure::column::Column::kOperatingVoltage
: superstructure::column::Column::kZeroingVoltage;
const double voltage_check_intake =
(static_cast<intake::Intake::State>(
- superstructure_status_fetcher_->intake.state) ==
+ superstructure_status_fetcher_->intake()->state()) ==
intake::Intake::State::RUNNING)
? superstructure::intake::Intake::kOperatingVoltage
: superstructure::intake::Intake::kZeroingVoltage;
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_hood),
+ AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_hood()),
voltage_check_hood);
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake),
+ AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_intake()),
voltage_check_intake);
- EXPECT_LE(::std::abs(superstructure_output_fetcher_->voltage_indexer),
+ EXPECT_LE(::std::abs(superstructure_output_fetcher_->voltage_indexer()),
voltage_check_indexer)
<< ": check voltage " << voltage_check_indexer;
- AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_turret),
+ AOS_CHECK_LE(::std::abs(superstructure_output_fetcher_->voltage_turret()),
voltage_check_turret);
::Eigen::Matrix<double, 1, 1> hood_U;
- hood_U << superstructure_output_fetcher_->voltage_hood +
+ hood_U << superstructure_output_fetcher_->voltage_hood() +
hood_plant_->voltage_offset();
::Eigen::Matrix<double, 1, 1> intake_U;
- intake_U << superstructure_output_fetcher_->voltage_intake +
+ intake_U << superstructure_output_fetcher_->voltage_intake() +
intake_plant_->voltage_offset();
::Eigen::Matrix<double, 1, 1> shooter_U;
- shooter_U << superstructure_output_fetcher_->voltage_shooter +
+ shooter_U << superstructure_output_fetcher_->voltage_shooter() +
shooter_plant_->voltage_offset();
::Eigen::Matrix<double, 2, 1> column_U;
- column_U << superstructure_output_fetcher_->voltage_indexer +
+ column_U << superstructure_output_fetcher_->voltage_indexer() +
column_plant_->indexer_voltage_offset(),
- superstructure_output_fetcher_->voltage_turret +
+ superstructure_output_fetcher_->voltage_turret() +
column_plant_->turret_voltage_offset();
hood_plant_->Update(hood_U);
@@ -465,9 +486,9 @@
::aos::EventLoop *event_loop_;
const chrono::nanoseconds dt_;
- ::aos::Sender<SuperstructureQueue::Position> superstructure_position_sender_;
- ::aos::Fetcher<SuperstructureQueue::Status> superstructure_status_fetcher_;
- ::aos::Fetcher<SuperstructureQueue::Output> superstructure_output_fetcher_;
+ ::aos::Sender<Position> superstructure_position_sender_;
+ ::aos::Fetcher<Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<Output> superstructure_output_fetcher_;
::std::unique_ptr<HoodPlant> hood_plant_;
PositionSensorSimulator hood_encoder_;
@@ -499,23 +520,20 @@
class SuperstructureTest : public ::aos::testing::ControlLoopTest {
protected:
SuperstructureTest()
- : ::aos::testing::ControlLoopTest(chrono::microseconds(5050)),
+ : ::aos::testing::ControlLoopTest(
+ aos::configuration::ReadConfig("y2017/config.json"),
+ chrono::microseconds(5050)),
test_event_loop_(MakeEventLoop()),
superstructure_goal_fetcher_(
- test_event_loop_->MakeFetcher<SuperstructureQueue::Goal>(
- ".y2017.control_loops.superstructure_queue.goal")),
+ test_event_loop_->MakeFetcher<Goal>("/superstructure")),
superstructure_goal_sender_(
- test_event_loop_->MakeSender<SuperstructureQueue::Goal>(
- ".y2017.control_loops.superstructure_queue.goal")),
+ test_event_loop_->MakeSender<Goal>("/superstructure")),
superstructure_status_fetcher_(
- test_event_loop_->MakeFetcher<SuperstructureQueue::Status>(
- ".y2017.control_loops.superstructure_queue.status")),
+ test_event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_output_fetcher_(
- test_event_loop_->MakeFetcher<SuperstructureQueue::Output>(
- ".y2017.control_loops.superstructure_queue.output")),
+ test_event_loop_->MakeFetcher<Output>("/superstructure")),
superstructure_position_fetcher_(
- test_event_loop_->MakeFetcher<SuperstructureQueue::Position>(
- ".y2017.control_loops.superstructure_queue.position")),
+ test_event_loop_->MakeFetcher<Position>("/superstructure")),
superstructure_event_loop_(MakeEventLoop()),
superstructure_(superstructure_event_loop_.get()),
superstructure_plant_event_loop_(MakeEventLoop()),
@@ -530,50 +548,49 @@
ASSERT_TRUE(superstructure_goal_fetcher_.get() != nullptr);
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
- EXPECT_NEAR(superstructure_goal_fetcher_->hood.angle,
- superstructure_status_fetcher_->hood.position, 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->hood.angle,
+ EXPECT_NEAR(superstructure_goal_fetcher_->hood()->angle(),
+ superstructure_status_fetcher_->hood()->position(), 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->hood()->angle(),
superstructure_plant_.hood_position(), 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->turret.angle,
- superstructure_status_fetcher_->turret.position, 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->turret.angle,
+ EXPECT_NEAR(superstructure_goal_fetcher_->turret()->angle(),
+ superstructure_status_fetcher_->turret()->position(), 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->turret()->angle(),
superstructure_plant_.turret_position(), 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->intake.distance,
- superstructure_status_fetcher_->intake.position, 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->intake.distance,
+ EXPECT_NEAR(superstructure_goal_fetcher_->intake()->distance(),
+ superstructure_status_fetcher_->intake()->position(), 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->intake()->distance(),
superstructure_plant_.intake_position(), 0.001);
// Check that the angular velocity, average angular velocity, and estimated
// angular velocity match when we are done for the shooter.
- EXPECT_NEAR(superstructure_goal_fetcher_->shooter.angular_velocity,
- superstructure_status_fetcher_->shooter.angular_velocity, 0.1);
- EXPECT_NEAR(superstructure_goal_fetcher_->shooter.angular_velocity,
- superstructure_status_fetcher_->shooter.avg_angular_velocity,
+ EXPECT_NEAR(superstructure_goal_fetcher_->shooter()->angular_velocity(),
+ superstructure_status_fetcher_->shooter()->angular_velocity(), 0.1);
+ EXPECT_NEAR(superstructure_goal_fetcher_->shooter()->angular_velocity(),
+ superstructure_status_fetcher_->shooter()->avg_angular_velocity(),
0.1);
- EXPECT_NEAR(superstructure_goal_fetcher_->shooter.angular_velocity,
+ EXPECT_NEAR(superstructure_goal_fetcher_->shooter()->angular_velocity(),
superstructure_plant_.shooter_velocity(), 0.1);
// Check that the angular velocity, average angular velocity, and estimated
// angular velocity match when we are done for the indexer.
- EXPECT_NEAR(superstructure_goal_fetcher_->indexer.angular_velocity,
- superstructure_status_fetcher_->indexer.angular_velocity, 0.1);
- EXPECT_NEAR(superstructure_goal_fetcher_->indexer.angular_velocity,
- superstructure_status_fetcher_->indexer.avg_angular_velocity,
+ EXPECT_NEAR(superstructure_goal_fetcher_->indexer()->angular_velocity(),
+ superstructure_status_fetcher_->indexer()->angular_velocity(), 0.1);
+ EXPECT_NEAR(superstructure_goal_fetcher_->indexer()->angular_velocity(),
+ superstructure_status_fetcher_->indexer()->avg_angular_velocity(),
0.1);
- EXPECT_NEAR(superstructure_goal_fetcher_->indexer.angular_velocity,
+ EXPECT_NEAR(superstructure_goal_fetcher_->indexer()->angular_velocity(),
superstructure_plant_.indexer_velocity(), 0.1);
}
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
- ::aos::Fetcher<SuperstructureQueue::Goal> superstructure_goal_fetcher_;
- ::aos::Sender<SuperstructureQueue::Goal> superstructure_goal_sender_;
- ::aos::Fetcher<SuperstructureQueue::Status> superstructure_status_fetcher_;
- ::aos::Fetcher<SuperstructureQueue::Output> superstructure_output_fetcher_;
- ::aos::Fetcher<SuperstructureQueue::Position>
- superstructure_position_fetcher_;
+ ::aos::Fetcher<Goal> superstructure_goal_fetcher_;
+ ::aos::Sender<Goal> superstructure_goal_sender_;
+ ::aos::Fetcher<Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<Output> superstructure_output_fetcher_;
+ ::aos::Fetcher<Position> superstructure_position_fetcher_;
// Create a control loop and simulation.
::std::unique_ptr<::aos::EventLoop> superstructure_event_loop_;
@@ -588,11 +605,36 @@
SetEnabled(true);
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = 0.2;
- goal->turret.angle = 0.0;
- goal->intake.distance = 0.05;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(0.2);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
+
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(0.0);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(0.05);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+ goal_builder.add_indexer(indexer_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
@@ -607,20 +649,48 @@
// Set a reasonable goal.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = 0.1;
- goal->hood.profile_params.max_velocity = 1;
- goal->hood.profile_params.max_acceleration = 0.5;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->turret.angle = 0.1;
- goal->turret.profile_params.max_velocity = 1;
- goal->turret.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<frc971::ProfileParameters>
+ hood_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(0.1);
+ hood_builder.add_profile_params(hood_profile_parameters_offset);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
- goal->intake.distance = 0.1;
- goal->intake.profile_params.max_velocity = 1;
- goal->intake.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<frc971::ProfileParameters>
+ turret_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(0.1);
+ turret_builder.add_profile_params(turret_profile_parameters_offset);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<frc971::ProfileParameters>
+ intake_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(0.1);
+ intake_builder.add_profile_params(intake_profile_parameters_offset);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+ goal_builder.add_indexer(indexer_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Give it a lot of time to get there.
@@ -638,11 +708,36 @@
// Zero it before we move.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.distance = constants::Values::kIntakeRange.upper;
- goal->turret.angle = constants::Values::kTurretRange.upper;
- goal->hood.angle = constants::Values::kHoodRange.upper;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(constants::Values::kHoodRange.upper);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
+
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(constants::Values::kTurretRange.upper);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(constants::Values::kIntakeRange.upper);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+ goal_builder.add_indexer(indexer_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(8));
VerifyNearGoal();
@@ -652,17 +747,48 @@
// Try a low acceleration move with a high max velocity and verify the
// acceleration is capped like expected.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.distance = constants::Values::kIntakeRange.lower;
- goal->intake.profile_params.max_velocity = 20.0;
- goal->intake.profile_params.max_acceleration = 0.1;
- goal->turret.angle = constants::Values::kTurretRange.lower;
- goal->turret.profile_params.max_velocity = 20.0;
- goal->turret.profile_params.max_acceleration = 1.0;
- goal->hood.angle = constants::Values::kHoodRange.lower;
- goal->hood.profile_params.max_velocity = 20.0;
- goal->hood.profile_params.max_acceleration = 1.0;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<frc971::ProfileParameters>
+ hood_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 20.0, 1.0);
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(constants::Values::kHoodRange.lower);
+ hood_builder.add_profile_params(hood_profile_parameters_offset);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
+
+ flatbuffers::Offset<frc971::ProfileParameters>
+ turret_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 20.0, 1.0);
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(constants::Values::kTurretRange.lower);
+ turret_builder.add_profile_params(turret_profile_parameters_offset);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ flatbuffers::Offset<frc971::ProfileParameters>
+ intake_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 20.0, 0.1);
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(constants::Values::kIntakeRange.lower);
+ intake_builder.add_profile_params(intake_profile_parameters_offset);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+ goal_builder.add_indexer(indexer_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_intake_velocity(23.0);
superstructure_plant_.set_peak_turret_velocity(23.0);
@@ -676,17 +802,48 @@
// Now do a high acceleration move with a low velocity limit.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.distance = constants::Values::kIntakeRange.upper;
- goal->intake.profile_params.max_velocity = 0.1;
- goal->intake.profile_params.max_acceleration = 100;
- goal->turret.angle = constants::Values::kTurretRange.upper;
- goal->turret.profile_params.max_velocity = 1;
- goal->turret.profile_params.max_acceleration = 100;
- goal->hood.angle = constants::Values::kHoodRange.upper;
- goal->hood.profile_params.max_velocity = 1;
- goal->hood.profile_params.max_acceleration = 100;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<frc971::ProfileParameters>
+ hood_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 100.0);
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(constants::Values::kHoodRange.upper);
+ hood_builder.add_profile_params(hood_profile_parameters_offset);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
+
+ flatbuffers::Offset<frc971::ProfileParameters>
+ turret_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 100.0);
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(constants::Values::kTurretRange.upper);
+ turret_builder.add_profile_params(turret_profile_parameters_offset);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ flatbuffers::Offset<frc971::ProfileParameters>
+ intake_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 0.1, 100.0);
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(constants::Values::kIntakeRange.upper);
+ intake_builder.add_profile_params(intake_profile_parameters_offset);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+ goal_builder.add_indexer(indexer_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
superstructure_plant_.set_peak_intake_velocity(0.2);
@@ -707,50 +864,106 @@
// Set some ridiculous goals to test upper limits.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = 100.0;
- goal->hood.profile_params.max_velocity = 1;
- goal->hood.profile_params.max_acceleration = 0.5;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->turret.angle = 100.0;
- goal->turret.profile_params.max_velocity = 1;
- goal->turret.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<frc971::ProfileParameters>
+ hood_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(100.0);
+ hood_builder.add_profile_params(hood_profile_parameters_offset);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
- goal->intake.distance = 100.0;
- goal->intake.profile_params.max_velocity = 1;
- goal->intake.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<frc971::ProfileParameters>
+ turret_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(100.0);
+ turret_builder.add_profile_params(turret_profile_parameters_offset);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<frc971::ProfileParameters>
+ intake_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(100.0);
+ intake_builder.add_profile_params(intake_profile_parameters_offset);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+ goal_builder.add_indexer(indexer_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
// Check that we are near our soft limit.
superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kHoodRange.upper,
- superstructure_status_fetcher_->hood.position, 0.001);
+ superstructure_status_fetcher_->hood()->position(), 0.001);
EXPECT_NEAR(constants::Values::kTurretRange.upper,
- superstructure_status_fetcher_->turret.position, 0.001);
+ superstructure_status_fetcher_->turret()->position(), 0.001);
EXPECT_NEAR(constants::Values::kIntakeRange.upper,
- superstructure_status_fetcher_->intake.position, 0.001);
+ superstructure_status_fetcher_->intake()->position(), 0.001);
// Set some ridiculous goals to test lower limits.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = -100.0;
- goal->hood.profile_params.max_velocity = 1;
- goal->hood.profile_params.max_acceleration = 0.5;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->turret.angle = -100.0;
- goal->turret.profile_params.max_velocity = 1;
- goal->turret.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<frc971::ProfileParameters>
+ hood_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(-100.0);
+ hood_builder.add_profile_params(hood_profile_parameters_offset);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
- goal->intake.distance = -100.0;
- goal->intake.profile_params.max_velocity = 1;
- goal->intake.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<frc971::ProfileParameters>
+ turret_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(-100.0);
+ turret_builder.add_profile_params(turret_profile_parameters_offset);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<frc971::ProfileParameters>
+ intake_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(-100.0);
+ intake_builder.add_profile_params(intake_profile_parameters_offset);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+ goal_builder.add_indexer(indexer_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -758,31 +971,59 @@
// Check that we are near our soft limit.
superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kHoodRange.lower,
- superstructure_status_fetcher_->hood.position, 0.001);
+ superstructure_status_fetcher_->hood()->position(), 0.001);
EXPECT_NEAR(constants::Values::kTurretRange.lower,
- superstructure_status_fetcher_->turret.position, 0.001);
+ superstructure_status_fetcher_->turret()->position(), 0.001);
EXPECT_NEAR(column::Column::kIntakeZeroingMinDistance,
- superstructure_status_fetcher_->intake.position, 0.001);
+ superstructure_status_fetcher_->intake()->position(), 0.001);
// Now, center the turret so we can try ridiculous things without having the
// intake pushed out.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = -100.0;
- goal->hood.profile_params.max_velocity = 1;
- goal->hood.profile_params.max_acceleration = 0.5;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->turret.angle = 0.0;
- goal->turret.profile_params.max_velocity = 1;
- goal->turret.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<frc971::ProfileParameters>
+ hood_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(-100.0);
+ hood_builder.add_profile_params(hood_profile_parameters_offset);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
- goal->intake.distance = -100.0;
- goal->intake.profile_params.max_velocity = 1;
- goal->intake.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<frc971::ProfileParameters>
+ turret_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(0.0);
+ turret_builder.add_profile_params(turret_profile_parameters_offset);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<frc971::ProfileParameters>
+ intake_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(-100.0);
+ intake_builder.add_profile_params(intake_profile_parameters_offset);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+ goal_builder.add_indexer(indexer_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -790,12 +1031,12 @@
// Check that we are near our soft limit.
superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kHoodRange.lower,
- superstructure_status_fetcher_->hood.position, 0.001);
+ superstructure_status_fetcher_->hood()->position(), 0.001);
- EXPECT_NEAR(0.0, superstructure_status_fetcher_->turret.position, 0.001);
+ EXPECT_NEAR(0.0, superstructure_status_fetcher_->turret()->position(), 0.001);
EXPECT_NEAR(constants::Values::kIntakeRange.lower,
- superstructure_status_fetcher_->intake.position, 0.001);
+ superstructure_status_fetcher_->intake()->position(), 0.001);
}
// Tests that the hood, turret and intake loops zeroes when run for a while.
@@ -803,19 +1044,48 @@
SetEnabled(true);
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = constants::Values::kHoodRange.lower;
- goal->hood.profile_params.max_velocity = 1;
- goal->hood.profile_params.max_acceleration = 0.5;
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->turret.angle = constants::Values::kTurretRange.lower;
- goal->turret.profile_params.max_velocity = 1;
- goal->turret.profile_params.max_acceleration = 0.5;
+ flatbuffers::Offset<frc971::ProfileParameters>
+ hood_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(constants::Values::kHoodRange.lower);
+ hood_builder.add_profile_params(hood_profile_parameters_offset);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
- goal->intake.distance = constants::Values::kIntakeRange.upper;
- goal->intake.profile_params.max_velocity = 1;
- goal->intake.profile_params.max_acceleration = 0.5;
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<frc971::ProfileParameters>
+ turret_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(constants::Values::kTurretRange.lower);
+ turret_builder.add_profile_params(turret_profile_parameters_offset);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ flatbuffers::Offset<frc971::ProfileParameters>
+ intake_profile_parameters_offset =
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 0.5);
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(constants::Values::kIntakeRange.upper);
+ intake_builder.add_profile_params(intake_profile_parameters_offset);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+ goal_builder.add_indexer(indexer_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -844,11 +1114,36 @@
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.lower_hard);
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = constants::Values::kHoodRange.lower;
- goal->turret.angle = constants::Values::kTurretRange.lower;
- goal->intake.distance = constants::Values::kIntakeRange.upper;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(constants::Values::kHoodRange.lower);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
+
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(constants::Values::kTurretRange.lower);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(constants::Values::kIntakeRange.upper);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+ goal_builder.add_indexer(indexer_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -867,11 +1162,36 @@
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.upper_hard);
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = constants::Values::kHoodRange.upper;
- goal->turret.angle = constants::Values::kTurretRange.upper;
- goal->intake.distance = constants::Values::kIntakeRange.upper;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(constants::Values::kHoodRange.upper);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
+
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(constants::Values::kTurretRange.upper);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(constants::Values::kIntakeRange.upper);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+ goal_builder.add_indexer(indexer_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -889,12 +1209,36 @@
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange.upper);
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = constants::Values::kHoodRange.upper - 0.1;
- goal->turret.angle = constants::Values::kTurretRange.upper - 0.1;
- goal->intake.distance = constants::Values::kIntakeRange.upper - 0.1;
- goal->indexer.angular_velocity = -5.0;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(constants::Values::kHoodRange.upper - 0.1);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
+
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(constants::Values::kTurretRange.upper - 0.1);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(constants::Values::kIntakeRange.upper - 0.1);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(-5.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+ goal_builder.add_indexer(indexer_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -923,15 +1267,42 @@
// Tests that the internal goals don't change while disabled.
TEST_F(SuperstructureTest, DisabledGoalTest) {
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = constants::Values::kHoodRange.lower + 0.03;
- goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
- goal->intake.distance = constants::Values::kIntakeRange.lower + 0.03;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(constants::Values::kHoodRange.lower + 0.03);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
+
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(constants::Values::kTurretRange.lower + 0.03);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(constants::Values::kIntakeRange.lower + 0.03);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+ goal_builder.add_indexer(indexer_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::milliseconds(100));
@@ -955,11 +1326,36 @@
constants::GetValues().hood.zeroing.measured_index_position - 0.001);
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = constants::Values::kHoodRange.lower;
- goal->turret.angle = 0.0;
- goal->intake.distance = constants::Values::kIntakeRange.lower;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(constants::Values::kHoodRange.lower);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
+
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(0.0);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(constants::Values::kIntakeRange.lower);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_shooter(shooter_offset);
+ goal_builder.add_indexer(indexer_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Run disabled for 2 seconds
@@ -1000,49 +1396,118 @@
// Spin up.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = constants::Values::kHoodRange.lower + 0.03;
- goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
- goal->intake.distance = constants::Values::kIntakeRange.upper - 0.03;
- goal->shooter.angular_velocity = 300.0;
- goal->indexer.angular_velocity = 20.0;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(constants::Values::kHoodRange.lower + 0.03);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
+
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(constants::Values::kTurretRange.lower + 0.03);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(constants::Values::kIntakeRange.upper - 0.03);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(300.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(20.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_indexer(indexer_offset);
+ goal_builder.add_shooter(shooter_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
VerifyNearGoal();
- EXPECT_TRUE(superstructure_status_fetcher_->shooter.ready);
+ EXPECT_TRUE(superstructure_status_fetcher_->shooter()->ready());
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = constants::Values::kHoodRange.lower + 0.03;
- goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
- goal->intake.distance = constants::Values::kIntakeRange.upper - 0.03;
- goal->shooter.angular_velocity = 0.0;
- goal->indexer.angular_velocity = 0.0;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(constants::Values::kHoodRange.lower + 0.03);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
+
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(constants::Values::kTurretRange.lower + 0.03);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(constants::Values::kIntakeRange.upper - 0.03);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_indexer(indexer_offset);
+ goal_builder.add_shooter(shooter_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Make sure we don't apply voltage on spin-down.
RunFor(dt());
EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
- EXPECT_EQ(0.0, superstructure_output_fetcher_->voltage_shooter);
+ EXPECT_EQ(0.0, superstructure_output_fetcher_->voltage_shooter());
// Continue to stop.
RunFor(chrono::seconds(5));
EXPECT_TRUE(superstructure_output_fetcher_.Fetch());
- EXPECT_EQ(0.0, superstructure_output_fetcher_->voltage_shooter);
+ EXPECT_EQ(0.0, superstructure_output_fetcher_->voltage_shooter());
}
// Tests that the shooter can spin up nicely after being disabled for a while.
TEST_F(SuperstructureTest, ShooterDisabled) {
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = constants::Values::kHoodRange.lower + 0.03;
- goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
- goal->intake.distance = constants::Values::kIntakeRange.upper - 0.03;
- goal->shooter.angular_velocity = 200.0;
- goal->indexer.angular_velocity = 20.0;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(constants::Values::kHoodRange.lower + 0.03);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
+
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(constants::Values::kTurretRange.lower + 0.03);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(constants::Values::kIntakeRange.upper - 0.03);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(200.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(20.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_indexer(indexer_offset);
+ goal_builder.add_shooter(shooter_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
EXPECT_EQ(nullptr, superstructure_output_fetcher_.get());
@@ -1059,18 +1524,41 @@
// Spin up.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = constants::Values::kHoodRange.lower + 0.03;
- goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
- goal->intake.distance = constants::Values::kIntakeRange.upper - 0.03;
- goal->shooter.angular_velocity = 0.0;
- goal->indexer.angular_velocity = 5.0;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(constants::Values::kHoodRange.lower + 0.03);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
+
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(constants::Values::kTurretRange.lower + 0.03);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(constants::Values::kIntakeRange.upper - 0.03);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(5.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_indexer(indexer_offset);
+ goal_builder.add_shooter(shooter_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
VerifyNearGoal();
- EXPECT_TRUE(superstructure_status_fetcher_->indexer.ready);
+ EXPECT_TRUE(superstructure_status_fetcher_->indexer()->ready());
// Now, stick it.
const auto stuck_start_time = monotonic_now();
@@ -1080,7 +1568,7 @@
superstructure_status_fetcher_.Fetch();
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
if (static_cast<column::Column::IndexerState>(
- superstructure_status_fetcher_->indexer.state) ==
+ superstructure_status_fetcher_->indexer()->state()) ==
column::Column::IndexerState::REVERSING) {
break;
}
@@ -1095,7 +1583,7 @@
superstructure_position_fetcher_.Fetch();
ASSERT_TRUE(superstructure_position_fetcher_.get() != nullptr);
const double indexer_position =
- superstructure_position_fetcher_->column.indexer.encoder;
+ superstructure_position_fetcher_->column()->indexer()->encoder();
// Now, unstick it.
superstructure_plant_.set_freeze_indexer(false);
@@ -1105,7 +1593,7 @@
superstructure_status_fetcher_.Fetch();
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
if (static_cast<column::Column::IndexerState>(
- superstructure_status_fetcher_->indexer.state) ==
+ superstructure_status_fetcher_->indexer()->state()) ==
column::Column::IndexerState::RUNNING) {
break;
}
@@ -1123,7 +1611,7 @@
superstructure_position_fetcher_.Fetch();
ASSERT_TRUE(superstructure_position_fetcher_.get() != nullptr);
const double unstuck_indexer_position =
- superstructure_position_fetcher_->column.indexer.encoder;
+ superstructure_position_fetcher_->column()->indexer()->encoder();
EXPECT_LT(unstuck_indexer_position, indexer_position - 0.1);
// Now, verify that everything works as expected.
@@ -1138,18 +1626,41 @@
// Spin up.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->hood.angle = constants::Values::kHoodRange.lower + 0.03;
- goal->turret.angle = constants::Values::kTurretRange.lower + 0.03;
- goal->intake.distance = constants::Values::kIntakeRange.upper - 0.03;
- goal->shooter.angular_velocity = 0.0;
- goal->indexer.angular_velocity = 5.0;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ HoodGoal::Builder hood_builder = builder.MakeBuilder<HoodGoal>();
+ hood_builder.add_angle(constants::Values::kHoodRange.lower + 0.03);
+ flatbuffers::Offset<HoodGoal> hood_offset = hood_builder.Finish();
+
+ TurretGoal::Builder turret_builder = builder.MakeBuilder<TurretGoal>();
+ turret_builder.add_angle(constants::Values::kTurretRange.lower + 0.03);
+ flatbuffers::Offset<TurretGoal> turret_offset = turret_builder.Finish();
+
+ IntakeGoal::Builder intake_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_builder.add_distance(constants::Values::kIntakeRange.upper - 0.03);
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_builder.Finish();
+
+ ShooterGoal::Builder shooter_builder = builder.MakeBuilder<ShooterGoal>();
+ shooter_builder.add_angular_velocity(0.0);
+ flatbuffers::Offset<ShooterGoal> shooter_offset = shooter_builder.Finish();
+
+ IndexerGoal::Builder indexer_builder = builder.MakeBuilder<IndexerGoal>();
+ indexer_builder.add_angular_velocity(5.0);
+ flatbuffers::Offset<IndexerGoal> indexer_offset = indexer_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_hood(hood_offset);
+ goal_builder.add_turret(turret_offset);
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_indexer(indexer_offset);
+ goal_builder.add_shooter(shooter_offset);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(5));
VerifyNearGoal();
- EXPECT_TRUE(superstructure_status_fetcher_->indexer.ready);
+ EXPECT_TRUE(superstructure_status_fetcher_->indexer()->ready());
// Now, stick it.
const auto stuck_start_time = monotonic_now();
@@ -1159,7 +1670,7 @@
superstructure_status_fetcher_.Fetch();
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
if (static_cast<column::Column::IndexerState>(
- superstructure_status_fetcher_->indexer.state) ==
+ superstructure_status_fetcher_->indexer()->state()) ==
column::Column::IndexerState::REVERSING) {
break;
}
@@ -1177,7 +1688,7 @@
superstructure_status_fetcher_.Fetch();
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
if (static_cast<column::Column::IndexerState>(
- superstructure_status_fetcher_->indexer.state) ==
+ superstructure_status_fetcher_->indexer()->state()) ==
column::Column::IndexerState::RUNNING) {
break;
}
@@ -1202,7 +1713,7 @@
superstructure_status_fetcher_.Fetch();
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
if (static_cast<column::Column::IndexerState>(
- superstructure_status_fetcher_->indexer.state) ==
+ superstructure_status_fetcher_->indexer()->state()) ==
column::Column::IndexerState::REVERSING) {
break;
}
diff --git a/y2017/control_loops/superstructure/superstructure_main.cc b/y2017/control_loops/superstructure/superstructure_main.cc
index 1dbb342..eedba6f 100644
--- a/y2017/control_loops/superstructure/superstructure_main.cc
+++ b/y2017/control_loops/superstructure/superstructure_main.cc
@@ -1,12 +1,15 @@
#include "y2017/control_loops/superstructure/superstructure.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;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
::y2017::control_loops::superstructure::Superstructure superstructure(
&event_loop);
diff --git a/y2017/control_loops/superstructure/superstructure_output.fbs b/y2017/control_loops/superstructure/superstructure_output.fbs
new file mode 100644
index 0000000..5301380
--- /dev/null
+++ b/y2017/control_loops/superstructure/superstructure_output.fbs
@@ -0,0 +1,27 @@
+namespace y2017.control_loops.superstructure;
+
+table Output {
+ // Voltages for some of the subsystems.
+ voltage_intake:double;
+ voltage_indexer:double;
+ voltage_shooter:double;
+
+ // Rollers on the intake.
+ voltage_intake_rollers:double;
+ // Roller on the indexer
+ voltage_indexer_rollers:double;
+
+ voltage_turret:double;
+ voltage_hood:double;
+
+ gear_servo:double;
+
+ // If true, the lights are on.
+ lights_on:bool;
+
+ red_light_on:bool;
+ green_light_on:bool;
+ blue_light_on:bool;
+}
+
+root_type Output;
diff --git a/y2017/control_loops/superstructure/superstructure_position.fbs b/y2017/control_loops/superstructure/superstructure_position.fbs
new file mode 100644
index 0000000..7f95462
--- /dev/null
+++ b/y2017/control_loops/superstructure/superstructure_position.fbs
@@ -0,0 +1,31 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2017.control_loops.superstructure;
+
+table ColumnPosition {
+ // Indexer angle in radians relative to the base. Positive is according to
+ // the right hand rule around +z.
+ indexer:frc971.HallEffectAndPosition;
+ // Turret angle in radians relative to the indexer. Positive is the same as
+ // the indexer.
+ turret:frc971.HallEffectAndPosition;
+}
+
+
+table Position {
+ // Position of the intake, zero when the intake is in, positive when it is
+ // out.
+ intake:frc971.PotAndAbsolutePosition;
+
+ // The position of the column.
+ column:ColumnPosition;
+
+ // The sensor readings for the hood. The units and sign are defined the
+ // same as what's in the Goal message.
+ hood:frc971.IndexPosition;
+
+ // Shooter wheel angle in radians.
+ theta_shooter:double;
+}
+
+root_type Position;
diff --git a/y2017/control_loops/superstructure/superstructure_status.fbs b/y2017/control_loops/superstructure/superstructure_status.fbs
new file mode 100644
index 0000000..217eb5b
--- /dev/null
+++ b/y2017/control_loops/superstructure/superstructure_status.fbs
@@ -0,0 +1,129 @@
+include "frc971/control_loops/control_loops.fbs";
+include "frc971/control_loops/profiled_subsystem.fbs";
+
+namespace y2017.control_loops.superstructure;
+
+table IndexerStatus {
+ // The current average velocity in radians/second. Positive is moving balls up
+ // towards the shooter. This is the angular velocity of the inner piece.
+ avg_angular_velocity:double;
+
+ // The current instantaneous filtered velocity in radians/second.
+ angular_velocity:double;
+
+ // True if the indexer is ready. It is better to compare the velocities
+ // directly so there isn't confusion on if the goal is up to date.
+ ready:bool;
+
+ // True if the indexer is stuck.
+ stuck:bool;
+ stuck_voltage:float;
+
+ // The state of the indexer state machine.
+ state:int;
+
+ // The estimated voltage error from the kalman filter in volts.
+ voltage_error:double;
+ // The estimated voltage error from the stuck indexer kalman filter.
+ stuck_voltage_error:double;
+
+ // The current velocity measured as delta x / delta t in radians/sec.
+ instantaneous_velocity:double;
+
+ // The error between our measurement and expected measurement in radians.
+ position_error:double;
+}
+
+table ShooterStatus {
+ // The current average velocity in radians/second.
+ avg_angular_velocity:double;
+
+ // The current instantaneous filtered velocity in radians/second.
+ angular_velocity:double;
+
+ // True if the shooter is ready. It is better to compare the velocities
+ // directly so there isn't confusion on if the goal is up to date.
+ ready:bool;
+
+ // The estimated voltage error from the kalman filter in volts.
+ voltage_error:double;
+
+ // The current velocity measured as delta x / delta t in radians/sec.
+ instantaneous_velocity:double;
+ filtered_velocity:double;
+ fixed_instantaneous_velocity:double;
+
+ // The error between our measurement and expected measurement in radians.
+ position_error:double;
+}
+
+table ColumnEstimatorState {
+ error:bool;
+ zeroed:bool;
+ indexer:frc971.HallEffectAndPositionEstimatorState;
+ turret:frc971.HallEffectAndPositionEstimatorState;
+}
+
+table TurretProfiledSubsystemStatus {
+ // Is the subsystem zeroed?
+ zeroed:bool;
+
+ // The state of the subsystem, if applicable. -1 otherwise.
+ state:int;
+
+ // If true, we have aborted.
+ estopped:bool;
+
+ // Position of the joint.
+ position:float;
+ // Velocity of the joint in units/second.
+ velocity:float;
+ // Profiled goal position of the joint.
+ goal_position:float;
+ // Profiled goal velocity of the joint in units/second.
+ goal_velocity:float;
+ // Unprofiled goal position from absoulte zero of the joint.
+ unprofiled_goal_position:float;
+ // Unprofiled goal velocity of the joint in units/second.
+ unprofiled_goal_velocity:float;
+
+ // The estimated voltage error.
+ voltage_error:float;
+
+ // The calculated velocity with delta x/delta t
+ calculated_velocity:float;
+
+ // Components of the control loop output
+ position_power:float;
+ velocity_power:float;
+ feedforwards_power:float;
+
+ // State of the estimator.
+ estimator_state:ColumnEstimatorState;
+
+ raw_vision_angle:double;
+ vision_angle:double;
+ vision_tracking:bool;
+
+ turret_encoder_angle:double;
+}
+
+table Status {
+ // Are all the subsystems zeroed?
+ zeroed:bool;
+
+ // If true, we have aborted. This is the or of all subsystem estops.
+ estopped:bool;
+
+ // Each subsystems status.
+ intake:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus;
+ hood:frc971.control_loops.IndexProfiledJointStatus;
+ shooter:ShooterStatus;
+
+ turret:TurretProfiledSubsystemStatus;
+ indexer:IndexerStatus;
+
+ vision_distance:float;
+}
+
+root_type Status;
diff --git a/y2017/control_loops/superstructure/vision_distance_average.h b/y2017/control_loops/superstructure/vision_distance_average.h
index 74e775d..7af2a9b 100644
--- a/y2017/control_loops/superstructure/vision_distance_average.h
+++ b/y2017/control_loops/superstructure/vision_distance_average.h
@@ -6,7 +6,7 @@
#include "aos/containers/ring_buffer.h"
#include "aos/time/time.h"
-#include "y2017/vision/vision.q.h"
+#include "y2017/vision/vision_generated.h"
namespace y2017 {
namespace control_loops {
@@ -28,8 +28,8 @@
cached_value_ = ComputeValue();
}
- if (vision_status != nullptr && vision_status->image_valid) {
- data_.Push({monotonic_now, vision_status->distance});
+ if (vision_status != nullptr && vision_status->image_valid()) {
+ data_.Push({monotonic_now, vision_status->distance()});
cached_value_ = ComputeValue();
}
}
diff --git a/y2017/control_loops/superstructure/vision_distance_average_test.cc b/y2017/control_loops/superstructure/vision_distance_average_test.cc
index 70f2f64..bc0fc9a 100644
--- a/y2017/control_loops/superstructure/vision_distance_average_test.cc
+++ b/y2017/control_loops/superstructure/vision_distance_average_test.cc
@@ -1,5 +1,6 @@
#include "y2017/control_loops/superstructure/vision_distance_average.h"
+#include "aos/flatbuffers.h"
#include "gtest/gtest.h"
namespace y2017 {
@@ -18,18 +19,28 @@
}
void TickInvalid() {
- vision::VisionStatus status;
- status.image_valid = false;
+ flatbuffers::FlatBufferBuilder fbb;
- average_.Tick(tick_time(), &status);
+ vision::VisionStatus::Builder status_builder(fbb);
+ status_builder.add_image_valid(false);
+ fbb.Finish(status_builder.Finish());
+
+ aos::FlatbufferDetachedBuffer<vision::VisionStatus> status(fbb.Release());
+
+ average_.Tick(tick_time(), &status.message());
}
void TickValid(double distance) {
- vision::VisionStatus status;
- status.image_valid = true;
- status.distance = distance;
+ flatbuffers::FlatBufferBuilder fbb;
- average_.Tick(tick_time(), &status);
+ vision::VisionStatus::Builder status_builder(fbb);
+ status_builder.add_image_valid(true);
+ status_builder.add_distance(distance);
+ fbb.Finish(status_builder.Finish());
+
+ aos::FlatbufferDetachedBuffer<vision::VisionStatus> status(fbb.Release());
+
+ average_.Tick(tick_time(), &status.message());
}
void TickNullptr() {
diff --git a/y2017/control_loops/superstructure/vision_time_adjuster.cc b/y2017/control_loops/superstructure/vision_time_adjuster.cc
index a67c5f2..7db653e 100644
--- a/y2017/control_loops/superstructure/vision_time_adjuster.cc
+++ b/y2017/control_loops/superstructure/vision_time_adjuster.cc
@@ -2,9 +2,8 @@
#include <chrono>
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2017/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2017/control_loops/superstructure/superstructure.q.h"
namespace y2017 {
namespace control_loops {
@@ -104,9 +103,8 @@
VisionTimeAdjuster::VisionTimeAdjuster(::aos::EventLoop *event_loop)
: drivetrain_status_fetcher_(
- event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Status>(
- ".frc971.control_loops.drivetrain_queue.status")) {}
+ event_loop->MakeFetcher<::frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")) {}
void VisionTimeAdjuster::Tick(monotonic_clock::time_point monotonic_now,
double turret_position,
@@ -117,18 +115,19 @@
// If we have new drivetrain data, we store it.
if (drivetrain_status_fetcher_.Fetch()) {
const auto &position = drivetrain_status_fetcher_.get();
- DrivetrainAngle new_position{.time = position->sent_time,
- .left = position->estimated_left_position,
- .right = position->estimated_right_position};
+ DrivetrainAngle new_position{
+ .time = drivetrain_status_fetcher_.context().monotonic_sent_time,
+ .left = position->estimated_left_position(),
+ .right = position->estimated_right_position()};
drivetrain_data_.Push(new_position);
most_recent_drivetrain_angle_ = ComputeDrivetrainPosition(new_position);
}
// If we have new vision data, compute the newest absolute angle at which the
// target is.
- if (vision_status != nullptr && vision_status->image_valid) {
+ if (vision_status != nullptr && vision_status->image_valid()) {
monotonic_clock::time_point last_target_time(
- monotonic_clock::duration(vision_status->target_time));
+ monotonic_clock::duration(vision_status->target_time()));
double column_angle = 0;
double drivetrain_angle = 0;
@@ -143,9 +142,9 @@
AOS_LOG(INFO, "Accepting Vision angle of %f, age %f\n",
most_recent_vision_angle_,
::aos::time::DurationInSeconds(monotonic_now - last_target_time));
- most_recent_vision_reading_ = vision_status->angle;
+ most_recent_vision_reading_ = vision_status->angle();
most_recent_vision_angle_ =
- vision_status->angle + column_angle + drivetrain_angle;
+ vision_status->angle() + column_angle + drivetrain_angle;
most_recent_vision_time_ = monotonic_now;
}
}
diff --git a/y2017/control_loops/superstructure/vision_time_adjuster.h b/y2017/control_loops/superstructure/vision_time_adjuster.h
index b45f827..3c873f7 100644
--- a/y2017/control_loops/superstructure/vision_time_adjuster.h
+++ b/y2017/control_loops/superstructure/vision_time_adjuster.h
@@ -4,10 +4,10 @@
#include <stdint.h>
#include "aos/containers/ring_buffer.h"
-#include "aos/events/event-loop.h"
+#include "aos/events/event_loop.h"
#include "aos/time/time.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
-#include "y2017/vision/vision.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "y2017/vision/vision_generated.h"
namespace y2017 {
namespace control_loops {
@@ -51,7 +51,7 @@
private:
// Fetcher to grab the latest drivetrain message.
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Status>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
// Buffer space to store the most recent drivetrain and turret messages from
diff --git a/y2017/control_loops/superstructure/vision_time_adjuster_test.cc b/y2017/control_loops/superstructure/vision_time_adjuster_test.cc
index 4f341de..7e9d82d 100644
--- a/y2017/control_loops/superstructure/vision_time_adjuster_test.cc
+++ b/y2017/control_loops/superstructure/vision_time_adjuster_test.cc
@@ -2,12 +2,13 @@
#include "gtest/gtest.h"
-#include "aos/events/simulated-event-loop.h"
+#include "aos/configuration.h"
+#include "aos/events/simulated_event_loop.h"
#include "aos/testing/test_logging.h"
#include "aos/time/time.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2017/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
-#include "y2017/vision/vision.q.h"
+#include "y2017/vision/vision_generated.h"
namespace y2017 {
namespace control_loops {
@@ -17,19 +18,20 @@
public:
VisionTimeAdjusterTest()
: ::testing::Test(),
- simulation_event_loop_(simulated_event_loop_factory_.MakeEventLoop()),
+ configuration_(aos::configuration::ReadConfig("y2017/config.json")),
+ event_loop_factory_(&configuration_.message()),
+ simulation_event_loop_(event_loop_factory_.MakeEventLoop()),
drivetrain_status_sender_(
simulation_event_loop_
- ->MakeSender<::frc971::control_loops::DrivetrainQueue::Status>(
- ".frc971.control_loops.drivetrain_queue.status")),
+ ->MakeSender<::frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")),
vision_status_sender_(
simulation_event_loop_->MakeSender<::y2017::vision::VisionStatus>(
- ".y2017.vision.vision_status")),
- vision_time_adjuster_event_loop_(
- simulated_event_loop_factory_.MakeEventLoop()),
- vision_status_fetcher_(vision_time_adjuster_event_loop_
- ->MakeFetcher<::y2017::vision::VisionStatus>(
- ".y2017.vision.vision_status")),
+ "/vision")),
+ vision_time_adjuster_event_loop_(event_loop_factory_.MakeEventLoop()),
+ vision_status_fetcher_(
+ vision_time_adjuster_event_loop_
+ ->MakeFetcher<::y2017::vision::VisionStatus>("/vision")),
adjuster_(vision_time_adjuster_event_loop_.get()) {
::aos::testing::EnableTestLogging();
static_assert(kVisionDelay % kTimeTick == ::std::chrono::milliseconds(0),
@@ -75,11 +77,11 @@
VisionTimeAdjuster *adjuster() { return &adjuster_; }
private:
- void TickTime() { simulated_event_loop_factory_.RunFor(kTimeTick); }
+ void TickTime() { event_loop_factory_.RunFor(kTimeTick); }
void SendMessages() {
SendDrivetrainPosition();
- if (simulated_event_loop_factory_.monotonic_now().time_since_epoch() %
+ if (event_loop_factory_.monotonic_now().time_since_epoch() %
kVisionTick ==
kVisionDelay) {
SendVisionTarget();
@@ -89,25 +91,33 @@
}
void SendDrivetrainPosition() {
- auto message = drivetrain_status_sender_.MakeMessage();
- message->estimated_left_position = drivetrain_left_;
- message->estimated_right_position = drivetrain_right_;
- ASSERT_TRUE(message.Send());
+ auto builder = drivetrain_status_sender_.MakeBuilder();
+
+ frc971::control_loops::drivetrain::Status::Builder status_builder =
+ builder.MakeBuilder<frc971::control_loops::drivetrain::Status>();
+ status_builder.add_estimated_left_position(drivetrain_left_);
+ status_builder.add_estimated_right_position(drivetrain_right_);
+
+ ASSERT_TRUE(builder.Send(status_builder.Finish()));
}
void SendVisionTarget() {
- auto message = vision_status_sender_.MakeMessage();
- message->target_time =
+ auto builder = vision_status_sender_.MakeBuilder();
+
+ vision::VisionStatus::Builder vision_status_builder =
+ builder.MakeBuilder<vision::VisionStatus>();
+ vision_status_builder.add_target_time(
(simulation_event_loop_->monotonic_now() - kVisionDelay)
.time_since_epoch()
- .count();
- message->distance = vision_distance_;
+ .count());
+ vision_status_builder.add_distance(vision_distance_);
ASSERT_EQ(turret_history_.capacity(), turret_history_.size());
ASSERT_EQ(drivetrain_history_.capacity(), drivetrain_history_.size());
- message->angle =
- vision_angle_ - turret_history_[0] - drivetrain_history_[0];
- message->image_valid = true;
- ASSERT_TRUE(message.Send());
+ vision_status_builder.add_angle(vision_angle_ - turret_history_[0] -
+ drivetrain_history_[0]);
+ vision_status_builder.add_image_valid(true);
+
+ ASSERT_TRUE(builder.Send(vision_status_builder.Finish()));
}
double GetDriveTrainAngle() const {
@@ -115,9 +125,10 @@
(drivetrain::kRobotRadius * 2.0);
}
- ::aos::SimulatedEventLoopFactory simulated_event_loop_factory_;
+ aos::FlatbufferDetachedBuffer<aos::Configuration> configuration_;
+ ::aos::SimulatedEventLoopFactory event_loop_factory_;
::std::unique_ptr<::aos::EventLoop> simulation_event_loop_;
- ::aos::Sender<::frc971::control_loops::DrivetrainQueue::Status>
+ ::aos::Sender<::frc971::control_loops::drivetrain::Status>
drivetrain_status_sender_;
::aos::Sender<::y2017::vision::VisionStatus> vision_status_sender_;