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/y2018/control_loops/superstructure/BUILD b/y2018/control_loops/superstructure/BUILD
index 5dabd1b..4e97571 100644
--- a/y2018/control_loops/superstructure/BUILD
+++ b/y2018/control_loops/superstructure/BUILD
@@ -1,18 +1,48 @@
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: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",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "superstructure_output_fbs",
+ srcs = [
+ "superstructure_output.fbs",
+ ],
+ gen_reflections = 1,
+)
+
cc_library(
name = "superstructure_lib",
srcs = [
@@ -22,16 +52,19 @@
"superstructure.h",
],
deps = [
- ":superstructure_queue",
+ ":superstructure_goal_fbs",
+ ":superstructure_output_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
"//aos/controls:control_loop",
- "//aos/events:event-loop",
- "//frc971/control_loops:queues",
- "//frc971/control_loops/drivetrain:drivetrain_queue",
+ "//aos/events:event_loop",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_output_fbs",
"//y2018:constants",
- "//y2018:status_light",
+ "//y2018:status_light_fbs",
"//y2018/control_loops/superstructure/arm",
"//y2018/control_loops/superstructure/intake",
- "//y2018/vision:vision_queue",
+ "//y2018/vision:vision_fbs",
],
)
@@ -41,12 +74,15 @@
srcs = [
"superstructure_lib_test.cc",
],
+ data = ["//y2018:config.json"],
shard_count = 5,
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",
@@ -63,7 +99,6 @@
],
deps = [
":superstructure_lib",
- ":superstructure_queue",
"//aos:init",
],
)
diff --git a/y2018/control_loops/superstructure/arm/BUILD b/y2018/control_loops/superstructure/arm/BUILD
index e8af2b9..bf3c306 100644
--- a/y2018/control_loops/superstructure/arm/BUILD
+++ b/y2018/control_loops/superstructure/arm/BUILD
@@ -12,7 +12,7 @@
"//aos/logging",
"//frc971/control_loops:dlqr",
"//frc971/control_loops:jacobian",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -27,7 +27,7 @@
":ekf",
":trajectory",
"//aos/testing:googletest",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -42,8 +42,8 @@
visibility = ["//visibility:public"],
deps = [
"//frc971/control_loops:runge_kutta",
- "//third_party/eigen",
"@com_github_gflags_gflags//:gflags",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -77,9 +77,9 @@
":ekf",
":generated_graph",
":trajectory",
- "//third_party/eigen",
"//third_party/matplotlib-cpp",
"@com_github_gflags_gflags//:gflags",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -95,7 +95,7 @@
deps = [
":dynamics",
"//frc971/control_loops:jacobian",
- "//third_party/eigen",
+ "@org_tuxfamily_eigen//:eigen",
],
)
@@ -129,10 +129,10 @@
":generated_graph",
":graph",
":trajectory",
- "//aos/logging:queue_logging",
"//frc971/zeroing",
"//y2018:constants",
- "//y2018/control_loops/superstructure:superstructure_queue",
+ "//y2018/control_loops/superstructure:superstructure_position_fbs",
+ "//y2018/control_loops/superstructure:superstructure_status_fbs",
],
)
diff --git a/y2018/control_loops/superstructure/arm/arm.cc b/y2018/control_loops/superstructure/arm/arm.cc
index 2ae1996..1f904e2 100644
--- a/y2018/control_loops/superstructure/arm/arm.cc
+++ b/y2018/control_loops/superstructure/arm/arm.cc
@@ -4,7 +4,6 @@
#include <iostream>
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/arm/demo_path.h"
#include "y2018/control_loops/superstructure/arm/dynamics.h"
@@ -44,15 +43,15 @@
void Arm::Reset() { state_ = State::UNINITIALIZED; }
-void Arm::Iterate(const ::aos::monotonic_clock::time_point monotonic_now,
- const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
- bool close_claw, const control_loops::ArmPosition *position,
- const bool claw_beambreak_triggered,
- const bool box_back_beambreak_triggered,
- const bool intake_clear_of_box, bool suicide,
- bool trajectory_override, double *proximal_output,
- double *distal_output, bool *release_arm_brake,
- bool *claw_closed, control_loops::ArmStatus *status) {
+flatbuffers::Offset<superstructure::ArmStatus> Arm::Iterate(
+ const ::aos::monotonic_clock::time_point monotonic_now,
+ const uint32_t *unsafe_goal, bool grab_box, bool open_claw, bool close_claw,
+ const superstructure::ArmPosition *position,
+ const bool claw_beambreak_triggered,
+ const bool box_back_beambreak_triggered, const bool intake_clear_of_box,
+ bool suicide, bool trajectory_override, double *proximal_output,
+ double *distal_output, bool *release_arm_brake, bool *claw_closed,
+ flatbuffers::FlatBufferBuilder *fbb) {
::Eigen::Matrix<double, 2, 1> Y;
const bool outputs_disabled =
((proximal_output == nullptr) || (distal_output == nullptr) ||
@@ -86,11 +85,11 @@
claw_closed_count_ = 50;
}
- Y << position->proximal.encoder + proximal_offset_,
- position->distal.encoder + distal_offset_;
+ Y << position->proximal()->encoder() + proximal_offset_,
+ position->distal()->encoder() + distal_offset_;
- proximal_zeroing_estimator_.UpdateEstimate(position->proximal);
- distal_zeroing_estimator_.UpdateEstimate(position->distal);
+ proximal_zeroing_estimator_.UpdateEstimate(*position->proximal());
+ distal_zeroing_estimator_.UpdateEstimate(*position->distal());
if (proximal_output != nullptr) {
*proximal_output = 0.0;
@@ -128,8 +127,8 @@
proximal_offset_ = proximal_zeroing_estimator_.offset();
distal_offset_ = distal_zeroing_estimator_.offset();
- Y << position->proximal.encoder + proximal_offset_,
- position->distal.encoder + distal_offset_;
+ Y << position->proximal()->encoder() + proximal_offset_,
+ position->distal()->encoder() + distal_offset_;
// TODO(austin): Offset ekf rather than reset it. Since we aren't
// moving at this point, it's pretty safe to do this.
@@ -377,17 +376,29 @@
follower_.Update(arm_ekf_.X_hat(), disable, kDt(), vmax_,
max_operating_voltage);
AOS_LOG(INFO, "Max voltage: %f\n", max_operating_voltage);
- status->goal_theta0 = follower_.theta(0);
- status->goal_theta1 = follower_.theta(1);
- status->goal_omega0 = follower_.omega(0);
- status->goal_omega1 = follower_.omega(1);
- status->theta0 = arm_ekf_.X_hat(0);
- status->theta1 = arm_ekf_.X_hat(2);
- status->omega0 = arm_ekf_.X_hat(1);
- status->omega1 = arm_ekf_.X_hat(3);
- status->voltage_error0 = arm_ekf_.X_hat(4);
- status->voltage_error1 = arm_ekf_.X_hat(5);
+ flatbuffers::Offset<frc971::PotAndAbsoluteEncoderEstimatorState>
+ proximal_estimator_state_offset =
+ proximal_zeroing_estimator_.GetEstimatorState(fbb);
+ flatbuffers::Offset<frc971::PotAndAbsoluteEncoderEstimatorState>
+ distal_estimator_state_offset =
+ distal_zeroing_estimator_.GetEstimatorState(fbb);
+
+ superstructure::ArmStatus::Builder status_builder(*fbb);
+ status_builder.add_proximal_estimator_state(proximal_estimator_state_offset);
+ status_builder.add_distal_estimator_state(distal_estimator_state_offset);
+
+ status_builder.add_goal_theta0(follower_.theta(0));
+ status_builder.add_goal_theta1(follower_.theta(1));
+ status_builder.add_goal_omega0(follower_.omega(0));
+ status_builder.add_goal_omega1(follower_.omega(1));
+
+ status_builder.add_theta0(arm_ekf_.X_hat(0));
+ status_builder.add_theta1(arm_ekf_.X_hat(2));
+ status_builder.add_omega0(arm_ekf_.X_hat(1));
+ status_builder.add_omega1(arm_ekf_.X_hat(3));
+ status_builder.add_voltage_error0(arm_ekf_.X_hat(4));
+ status_builder.add_voltage_error1(arm_ekf_.X_hat(5));
if (!disable) {
*proximal_output = ::std::max(
@@ -402,22 +413,17 @@
*claw_closed = claw_closed_;
}
- status->proximal_estimator_state =
- proximal_zeroing_estimator_.GetEstimatorState();
- status->distal_estimator_state =
- distal_zeroing_estimator_.GetEstimatorState();
+ status_builder.add_path_distance_to_go(follower_.path_distance_to_go());
+ status_builder.add_current_node(current_node_);
- status->path_distance_to_go = follower_.path_distance_to_go();
- status->current_node = current_node_;
-
- status->zeroed = (proximal_zeroing_estimator_.zeroed() &&
- distal_zeroing_estimator_.zeroed());
- status->estopped = (state_ == State::ESTOP);
- status->state = static_cast<int32_t>(state_);
- status->grab_state = static_cast<int32_t>(grab_state_);
- status->failed_solutions = follower_.failed_solutions();
+ status_builder.add_zeroed(zeroed());
+ status_builder.add_estopped(estopped());
+ status_builder.add_state(static_cast<int32_t>(state_));
+ status_builder.add_grab_state(static_cast<int32_t>(grab_state_));
+ status_builder.add_failed_solutions(follower_.failed_solutions());
arm_ekf_.Predict(follower_.U(), kDt());
+ return status_builder.Finish();
}
} // namespace arm
diff --git a/y2018/control_loops/superstructure/arm/arm.h b/y2018/control_loops/superstructure/arm/arm.h
index 8cd6b39..7ceb001 100644
--- a/y2018/control_loops/superstructure/arm/arm.h
+++ b/y2018/control_loops/superstructure/arm/arm.h
@@ -1,6 +1,7 @@
#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_ARM_H_
#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_ARM_H_
+#include "aos/time/time.h"
#include "frc971/zeroing/zeroing.h"
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/arm/dynamics.h"
@@ -8,7 +9,8 @@
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
#include "y2018/control_loops/superstructure/arm/graph.h"
#include "y2018/control_loops/superstructure/arm/trajectory.h"
-#include "y2018/control_loops/superstructure/superstructure.q.h"
+#include "y2018/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2018/control_loops/superstructure/superstructure_status_generated.h"
namespace y2018 {
namespace control_loops {
@@ -34,15 +36,15 @@
static constexpr double kPathlessVMax() { return 5.0; }
static constexpr double kGotoPathVMax() { return 6.0; }
- void Iterate(const ::aos::monotonic_clock::time_point monotonic_now,
- const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
- bool close_claw, const control_loops::ArmPosition *position,
- const bool claw_beambreak_triggered,
- const bool box_back_beambreak_triggered,
- const bool intake_clear_of_box, bool suicide,
- bool trajectory_override, double *proximal_output,
- double *distal_output, bool *release_arm_brake,
- bool *claw_closed, control_loops::ArmStatus *status);
+ flatbuffers::Offset<superstructure::ArmStatus> Iterate(
+ const ::aos::monotonic_clock::time_point monotonic_now,
+ const uint32_t *unsafe_goal, bool grab_box, bool open_claw,
+ bool close_claw, const superstructure::ArmPosition *position,
+ const bool claw_beambreak_triggered,
+ const bool box_back_beambreak_triggered, const bool intake_clear_of_box,
+ bool suicide, bool trajectory_override, double *proximal_output,
+ double *distal_output, bool *release_arm_brake, bool *claw_closed,
+ flatbuffers::FlatBufferBuilder *fbb);
void Reset();
@@ -68,6 +70,12 @@
State state() const { return state_; }
GrabState grab_state() const { return grab_state_; }
+ bool estopped() const { return state_ == State::ESTOP; }
+ bool zeroed() const {
+ return (proximal_zeroing_estimator_.zeroed() &&
+ distal_zeroing_estimator_.zeroed());
+ }
+
// Returns the maximum position for the intake. This is used to override the
// intake position to release the box when the state machine is lifting.
double max_intake_override() const { return max_intake_override_; }
diff --git a/y2018/control_loops/superstructure/intake/BUILD b/y2018/control_loops/superstructure/intake/BUILD
index ca1289e..dde7c63 100644
--- a/y2018/control_loops/superstructure/intake/BUILD
+++ b/y2018/control_loops/superstructure/intake/BUILD
@@ -41,10 +41,11 @@
":intake_plants",
"//aos:math",
"//aos/controls:control_loop",
- "//aos/logging:queue_logging",
- "//frc971/control_loops:queues",
+ "//frc971/control_loops:control_loops_fbs",
"//frc971/zeroing",
"//y2018:constants",
- "//y2018/control_loops/superstructure:superstructure_queue",
+ "//y2018/control_loops/superstructure:superstructure_output_fbs",
+ "//y2018/control_loops/superstructure:superstructure_position_fbs",
+ "//y2018/control_loops/superstructure:superstructure_status_fbs",
],
)
diff --git a/y2018/control_loops/superstructure/intake/intake.cc b/y2018/control_loops/superstructure/intake/intake.cc
index 6c07a49..dffebbc 100644
--- a/y2018/control_loops/superstructure/intake/intake.cc
+++ b/y2018/control_loops/superstructure/intake/intake.cc
@@ -3,9 +3,7 @@
#include <chrono>
#include "aos/commonmath.h"
-#include "aos/controls/control_loops.q.h"
#include "aos/logging/logging.h"
-#include "aos/logging/queue_logging.h"
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/intake/intake_delayed_plant.h"
@@ -80,15 +78,15 @@
loop_->Update(disabled);
}
-void IntakeController::SetStatus(IntakeSideStatus *status,
+void IntakeController::SetStatus(IntakeSideStatus::Builder *status,
const double *unsafe_goal) {
- status->goal_position = goal_angle(unsafe_goal);
- status->goal_velocity = loop_->R(1, 0);
- status->spring_position = loop_->X_hat(0) - loop_->X_hat(2);
- status->spring_velocity = loop_->X_hat(1) - loop_->X_hat(3);
- status->motor_position = loop_->X_hat(2);
- status->motor_velocity = loop_->X_hat(3);
- status->delayed_voltage = loop_->X_hat(4);
+ status->add_goal_position(goal_angle(unsafe_goal));
+ status->add_goal_velocity(loop_->R(1, 0));
+ status->add_spring_position(loop_->X_hat(0) - loop_->X_hat(2));
+ status->add_spring_velocity(loop_->X_hat(1) - loop_->X_hat(3));
+ status->add_motor_position(loop_->X_hat(2));
+ status->add_motor_velocity(loop_->X_hat(3));
+ status->add_delayed_voltage(loop_->X_hat(4));
}
IntakeSide::IntakeSide(
@@ -98,11 +96,12 @@
void IntakeSide::Reset() { state_ = State::UNINITIALIZED; }
-void IntakeSide::Iterate(const double *unsafe_goal,
- const control_loops::IntakeElasticSensors *position,
- control_loops::IntakeVoltage *output,
- control_loops::IntakeSideStatus *status) {
- zeroing_estimator_.UpdateEstimate(position->motor_position);
+flatbuffers::Offset<superstructure::IntakeSideStatus> IntakeSide::Iterate(
+ const double *unsafe_goal,
+ const superstructure::IntakeElasticSensors *position,
+ superstructure::IntakeVoltageT *output,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ zeroing_estimator_.UpdateEstimate(*position->motor_position());
switch (state_) {
case State::UNINITIALIZED:
@@ -132,8 +131,8 @@
state_ = State::UNINITIALIZED;
}
// ESTOP if we hit the hard limits.
- if ((status->motor_position) > controller_.intake_range_.upper ||
- (status->motor_position) < controller_.intake_range_.lower) {
+ if ((controller_.motor_position()) > controller_.intake_range().upper_hard ||
+ (controller_.motor_position()) < controller_.intake_range().lower_hard) {
AOS_LOG(ERROR, "Hit hard limits\n");
state_ = State::ESTOP;
}
@@ -145,8 +144,8 @@
}
const bool disable = (output == nullptr) || state_ != State::RUNNING;
- controller_.set_position(position->spring_angle,
- position->motor_position.encoder);
+ controller_.set_position(position->spring_angle(),
+ position->motor_position()->encoder());
controller_.Update(disable, unsafe_goal);
@@ -154,16 +153,25 @@
output->voltage_elastic = controller_.voltage();
}
+ flatbuffers::Offset<frc971::PotAndAbsoluteEncoderEstimatorState>
+ estimator_state = zeroing_estimator_.GetEstimatorState(fbb);
+
+ superstructure::IntakeSideStatus::Builder status_builder(*fbb);
// Save debug/internal state.
- status->estimator_state = zeroing_estimator_.GetEstimatorState();
- controller_.SetStatus(status, unsafe_goal);
- status->calculated_velocity =
- (status->estimator_state.position - intake_last_position_) /
- controller_.kDt;
- status->zeroed = zeroing_estimator_.zeroed();
- status->estopped = (state_ == State::ESTOP);
- status->state = static_cast<int32_t>(state_);
- intake_last_position_ = status->estimator_state.position;
+ status_builder.add_estimator_state(estimator_state);
+
+ controller_.SetStatus(&status_builder, unsafe_goal);
+ status_builder.add_calculated_velocity(
+ (zeroing_estimator_.offset() + position->motor_position()->encoder() -
+ intake_last_position_) /
+ controller_.kDt);
+ status_builder.add_zeroed(zeroing_estimator_.zeroed());
+ status_builder.add_estopped(estopped());
+ status_builder.add_state ( static_cast<int32_t>(state_));
+ intake_last_position_ =
+ zeroing_estimator_.offset() + position->motor_position()->encoder();
+
+ return status_builder.Finish();
}
} // namespace intake
diff --git a/y2018/control_loops/superstructure/intake/intake.h b/y2018/control_loops/superstructure/intake/intake.h
index c3d9772..7f0ec9f 100644
--- a/y2018/control_loops/superstructure/intake/intake.h
+++ b/y2018/control_loops/superstructure/intake/intake.h
@@ -3,12 +3,13 @@
#include "aos/commonmath.h"
#include "aos/controls/control_loop.h"
-#include "frc971/control_loops/control_loops.q.h"
#include "frc971/zeroing/zeroing.h"
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/intake/intake_delayed_plant.h"
#include "y2018/control_loops/superstructure/intake/intake_plant.h"
-#include "y2018/control_loops/superstructure/superstructure.q.h"
+#include "y2018/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2018/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2018/control_loops/superstructure/superstructure_status_generated.h"
namespace y2018 {
namespace control_loops {
@@ -23,13 +24,14 @@
void set_position(double spring_angle, double output_position);
// Populates the status structure.
- void SetStatus(control_loops::IntakeSideStatus *status,
+ void SetStatus(superstructure::IntakeSideStatus::Builder *status,
const double *unsafe_goal);
// Returns the control loop calculated voltage.
double voltage() const;
double output_position() const { return loop_->X_hat(0); }
+ double motor_position() const { return loop_->X_hat(2); }
// Executes the control loop for a cycle.
void Update(bool disabled, const double *unsafe_goal);
@@ -40,12 +42,6 @@
// Sets the goal angle from unsafe_goal.
double goal_angle(const double *unsafe_goal);
- // The control loop.
- ::std::unique_ptr<
- StateFeedbackLoop<5, 1, 2, double, StateFeedbackPlant<5, 1, 2>,
- StateFeedbackObserver<5, 1, 2>>>
- loop_;
-
constexpr static double kDt =
::aos::time::DurationInSeconds(::aos::controls::kLoopFrequency);
@@ -53,12 +49,22 @@
// possible otherwise zero.
void UpdateOffset(double offset);
+ const ::frc971::constants::Range intake_range() const {
+ return intake_range_;
+ }
+
+ private:
+ // The control loop.
+ ::std::unique_ptr<
+ StateFeedbackLoop<5, 1, 2, double, StateFeedbackPlant<5, 1, 2>,
+ StateFeedbackObserver<5, 1, 2>>>
+ loop_;
+
const ::frc971::constants::Range intake_range_;
// Stores the current zeroing estimator offset.
double offset_ = 0.0;
- private:
bool reset_ = true;
// The current sensor measurement.
@@ -75,10 +81,11 @@
// The operating voltage.
static constexpr double kOperatingVoltage() { return 12.0; }
- void Iterate(const double *unsafe_goal,
- const control_loops::IntakeElasticSensors *position,
- control_loops::IntakeVoltage *output,
- control_loops::IntakeSideStatus *status);
+ flatbuffers::Offset<superstructure::IntakeSideStatus> Iterate(
+ const double *unsafe_goal,
+ const superstructure::IntakeElasticSensors *position,
+ superstructure::IntakeVoltageT *output,
+ flatbuffers::FlatBufferBuilder *fbb);
void Reset();
@@ -91,6 +98,9 @@
State state() const { return state_; }
+ bool estopped() const { return state_ == State::ESTOP; }
+ bool zeroed() const { return zeroing_estimator_.zeroed(); }
+
bool clear_of_box() const { return controller_.output_position() < -0.1; }
double output_position() const { return controller_.output_position(); }
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index a2dcaf6..70af274 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -2,14 +2,13 @@
#include <chrono>
-#include "aos/controls/control_loops.q.h"
#include "aos/logging/logging.h"
-#include "frc971/control_loops/control_loops.q.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/intake/intake.h"
-#include "y2018/status_light.q.h"
-#include "y2018/vision/vision.q.h"
+#include "y2018/status_light_generated.h"
+#include "y2018/vision/vision_generated.h"
namespace y2018 {
namespace control_loops {
@@ -26,25 +25,23 @@
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),
status_light_sender_(
- event_loop->MakeSender<::y2018::StatusLight>(".y2018.status_light")),
+ event_loop->MakeSender<::y2018::StatusLight>("/superstructure")),
vision_status_fetcher_(
event_loop->MakeFetcher<::y2018::vision::VisionStatus>(
- ".y2018.vision.vision_status")),
+ "/superstructure")),
drivetrain_output_fetcher_(
- event_loop
- ->MakeFetcher<::frc971::control_loops::DrivetrainQueue::Output>(
- ".frc971.control_loops.drivetrain_queue.output")),
+ event_loop->MakeFetcher<::frc971::control_loops::drivetrain::Output>(
+ "/drivetrain")),
intake_left_(constants::GetValues().left_intake.zeroing),
intake_right_(constants::GetValues().right_intake.zeroing) {}
-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) {
+void Superstructure::RunIteration(const Goal *unsafe_goal,
+ const Position *position,
+ aos::Sender<Output>::Builder *output,
+ aos::Sender<Status>::Builder *status) {
const monotonic_clock::time_point monotonic_now =
event_loop()->monotonic_now();
if (WasReset()) {
@@ -55,7 +52,7 @@
}
const double clipped_box_distance =
- ::std::min(1.0, ::std::max(0.0, position->box_distance));
+ ::std::min(1.0, ::std::max(0.0, position->box_distance()));
const double box_velocity =
(clipped_box_distance - last_box_distance_) / 0.005;
@@ -64,31 +61,34 @@
filtered_box_velocity_ =
box_velocity * kFilteredBoxVelocityAlpha +
(1.0 - kFilteredBoxVelocityAlpha) * filtered_box_velocity_;
- status->filtered_box_velocity = filtered_box_velocity_;
constexpr double kCenteringAngleGain = 0.0;
const double left_intake_goal =
- ::std::min(
- arm_.max_intake_override(),
- (unsafe_goal == nullptr ? 0.0
- : unsafe_goal->intake.left_intake_angle)) +
+ ::std::min(arm_.max_intake_override(),
+ (unsafe_goal == nullptr || !unsafe_goal->has_intake()
+ ? 0.0
+ : unsafe_goal->intake()->left_intake_angle())) +
last_intake_center_error_ * kCenteringAngleGain;
const double right_intake_goal =
- ::std::min(
- arm_.max_intake_override(),
- (unsafe_goal == nullptr ? 0.0
- : unsafe_goal->intake.right_intake_angle)) -
+ ::std::min(arm_.max_intake_override(),
+ (unsafe_goal == nullptr || !unsafe_goal->has_intake()
+ ? 0.0
+ : unsafe_goal->intake()->right_intake_angle())) -
last_intake_center_error_ * kCenteringAngleGain;
- intake_left_.Iterate(unsafe_goal != nullptr ? &(left_intake_goal) : nullptr,
- &(position->left_intake),
- output != nullptr ? &(output->left_intake) : nullptr,
- &(status->left_intake));
+ IntakeVoltageT left_intake_output;
+ flatbuffers::Offset<superstructure::IntakeSideStatus> left_status_offset =
+ intake_left_.Iterate(
+ unsafe_goal != nullptr ? &(left_intake_goal) : nullptr,
+ position->left_intake(),
+ output != nullptr ? &(left_intake_output) : nullptr, status->fbb());
- intake_right_.Iterate(unsafe_goal != nullptr ? &(right_intake_goal) : nullptr,
- &(position->right_intake),
- output != nullptr ? &(output->right_intake) : nullptr,
- &(status->right_intake));
+ IntakeVoltageT right_intake_output;
+ flatbuffers::Offset<superstructure::IntakeSideStatus> right_status_offset =
+ intake_right_.Iterate(
+ unsafe_goal != nullptr ? &(right_intake_goal) : nullptr,
+ position->right_intake(),
+ output != nullptr ? &(right_intake_output) : nullptr, status->fbb());
const double intake_center_error =
intake_right_.output_position() - intake_left_.output_position();
@@ -97,63 +97,81 @@
const bool intake_clear_of_box =
intake_left_.clear_of_box() && intake_right_.clear_of_box();
- bool open_claw = unsafe_goal != nullptr ? unsafe_goal->open_claw : false;
+ bool open_claw = unsafe_goal != nullptr ? unsafe_goal->open_claw() : false;
if (unsafe_goal) {
- if (unsafe_goal->open_threshold != 0.0) {
- if (arm_.current_node() != unsafe_goal->arm_goal_position ||
- arm_.path_distance_to_go() > unsafe_goal->open_threshold) {
+ if (unsafe_goal->open_threshold() != 0.0) {
+ if (arm_.current_node() != unsafe_goal->arm_goal_position() ||
+ arm_.path_distance_to_go() > unsafe_goal->open_threshold()) {
open_claw = false;
}
}
}
- arm_.Iterate(
- monotonic_now,
- unsafe_goal != nullptr ? &(unsafe_goal->arm_goal_position) : nullptr,
- unsafe_goal != nullptr ? unsafe_goal->grab_box : false, open_claw,
- unsafe_goal != nullptr ? unsafe_goal->close_claw : false,
- &(position->arm), position->claw_beambreak_triggered,
- position->box_back_beambreak_triggered, intake_clear_of_box,
- unsafe_goal != nullptr ? unsafe_goal->voltage_winch > 1.0 : false,
- unsafe_goal != nullptr ? unsafe_goal->trajectory_override : false,
- output != nullptr ? &(output->voltage_proximal) : nullptr,
- output != nullptr ? &(output->voltage_distal) : nullptr,
- output != nullptr ? &(output->release_arm_brake) : nullptr,
- output != nullptr ? &(output->claw_grabbed) : nullptr, &(status->arm));
+ const uint32_t arm_goal_position =
+ unsafe_goal != nullptr ? unsafe_goal->arm_goal_position() : 0u;
+
+ double voltage_proximal_output = 0.0;
+ double voltage_distal_output = 0.0;
+ bool release_arm_brake_output = false;
+ bool claw_grabbed_output = false;
+ flatbuffers::Offset<superstructure::ArmStatus> arm_status_offset =
+ arm_.Iterate(
+ monotonic_now,
+ unsafe_goal != nullptr ? &(arm_goal_position) : nullptr,
+ unsafe_goal != nullptr ? unsafe_goal->grab_box() : false, open_claw,
+ unsafe_goal != nullptr ? unsafe_goal->close_claw() : false,
+ position->arm(), position->claw_beambreak_triggered(),
+ position->box_back_beambreak_triggered(), intake_clear_of_box,
+ unsafe_goal != nullptr ? unsafe_goal->voltage_winch() > 1.0 : false,
+ unsafe_goal != nullptr ? unsafe_goal->trajectory_override() : false,
+ output != nullptr ? &voltage_proximal_output : nullptr,
+ output != nullptr ? &voltage_distal_output : nullptr,
+ output != nullptr ? &release_arm_brake_output : nullptr,
+ output != nullptr ? &claw_grabbed_output : nullptr, status->fbb());
+
+
+ bool hook_release_output = false;
+ bool forks_release_output = false;
+ double voltage_winch_output = 0.0;
if (output) {
if (unsafe_goal) {
- output->hook_release = unsafe_goal->hook_release;
- output->voltage_winch = unsafe_goal->voltage_winch;
- output->forks_release = unsafe_goal->deploy_fork;
- } else {
- output->voltage_winch = 0.0;
- output->hook_release = false;
- output->forks_release = false;
+ hook_release_output = unsafe_goal->hook_release();
+ voltage_winch_output = unsafe_goal->voltage_winch();
+ forks_release_output = unsafe_goal->deploy_fork();
}
}
- status->estopped = status->left_intake.estopped ||
- status->right_intake.estopped || status->arm.estopped;
+ Status::Builder status_builder = status->MakeBuilder<Status>();
- status->zeroed = status->left_intake.zeroed && status->right_intake.zeroed &&
- status->arm.zeroed;
+ status_builder.add_left_intake(left_status_offset);
+ status_builder.add_right_intake(right_status_offset);
+ status_builder.add_arm(arm_status_offset);
+
+ status_builder.add_filtered_box_velocity(filtered_box_velocity_);
+ const bool estopped =
+ intake_left_.estopped() || intake_right_.estopped() || arm_.estopped();
+ status_builder.add_estopped(estopped);
+
+ status_builder.add_zeroed(intake_left_.zeroed() && intake_right_.zeroed() &&
+ arm_.zeroed());
if (output && unsafe_goal) {
- double roller_voltage = ::std::max(
- -kMaxIntakeRollerVoltage, ::std::min(unsafe_goal->intake.roller_voltage,
- kMaxIntakeRollerVoltage));
+ double roller_voltage =
+ ::std::max(-kMaxIntakeRollerVoltage,
+ ::std::min(unsafe_goal->intake()->roller_voltage(),
+ kMaxIntakeRollerVoltage));
constexpr int kReverseTime = 14;
- if (unsafe_goal->intake.roller_voltage < 0.0 ||
- unsafe_goal->disable_box_correct) {
- output->left_intake.voltage_rollers = roller_voltage;
- output->right_intake.voltage_rollers = roller_voltage;
+ if (unsafe_goal->intake()->roller_voltage() < 0.0 ||
+ unsafe_goal->disable_box_correct()) {
+ left_intake_output.voltage_rollers = roller_voltage;
+ right_intake_output.voltage_rollers = roller_voltage;
rotation_state_ = RotationState::NOT_ROTATING;
rotation_count_ = 0;
stuck_count_ = 0;
} else {
- const bool stuck = position->box_distance < 0.20 &&
- filtered_box_velocity_ > -0.05 &&
- !position->box_back_beambreak_triggered;
+ const bool stuck = position->box_distance() < 0.20 &&
+ filtered_box_velocity_ > -0.05 &&
+ !position->box_back_beambreak_triggered();
// Make sure we don't declare ourselves re-stuck too quickly. We want to
// wait 400 ms before triggering the stuck condition again.
if (!stuck) {
@@ -171,11 +189,11 @@
rotation_state_ = RotationState::STUCK;
++stuck_count_;
last_stuck_time_ = monotonic_now;
- } else if (position->left_intake.beam_break) {
+ } else if (position->left_intake()->beam_break()) {
rotation_state_ = RotationState::ROTATING_RIGHT;
rotation_count_ = kReverseTime;
break;
- } else if (position->right_intake.beam_break) {
+ } else if (position->right_intake()->beam_break()) {
rotation_state_ = RotationState::ROTATING_LEFT;
rotation_count_ = kReverseTime;
break;
@@ -190,7 +208,7 @@
}
} break;
case RotationState::ROTATING_LEFT:
- if (position->right_intake.beam_break) {
+ if (position->right_intake()->beam_break()) {
rotation_count_ = kReverseTime;
} else {
--rotation_count_;
@@ -200,7 +218,7 @@
}
break;
case RotationState::ROTATING_RIGHT:
- if (position->left_intake.beam_break) {
+ if (position->left_intake()->beam_break()) {
rotation_count_ = kReverseTime;
} else {
--rotation_count_;
@@ -214,7 +232,7 @@
constexpr double kHoldVoltage = 1.0;
constexpr double kStuckVoltage = 10.0;
- if (position->box_back_beambreak_triggered &&
+ if (position->box_back_beambreak_triggered() &&
roller_voltage > kHoldVoltage) {
roller_voltage = kHoldVoltage;
}
@@ -226,31 +244,31 @@
centering_gain = 0.0;
}
}
- output->left_intake.voltage_rollers =
+ left_intake_output.voltage_rollers =
roller_voltage - intake_center_error * centering_gain;
- output->right_intake.voltage_rollers =
+ right_intake_output.voltage_rollers =
roller_voltage + intake_center_error * centering_gain;
} break;
case RotationState::STUCK: {
if (roller_voltage > kHoldVoltage) {
- output->left_intake.voltage_rollers = -kStuckVoltage;
- output->right_intake.voltage_rollers = -kStuckVoltage;
+ left_intake_output.voltage_rollers = -kStuckVoltage;
+ right_intake_output.voltage_rollers = -kStuckVoltage;
}
} break;
case RotationState::ROTATING_LEFT:
- if (position->left_intake.beam_break) {
- output->left_intake.voltage_rollers = -roller_voltage * 0.9;
+ if (position->left_intake()->beam_break()) {
+ left_intake_output.voltage_rollers = -roller_voltage * 0.9;
} else {
- output->left_intake.voltage_rollers = -roller_voltage * 0.6;
+ left_intake_output.voltage_rollers = -roller_voltage * 0.6;
}
- output->right_intake.voltage_rollers = roller_voltage;
+ right_intake_output.voltage_rollers = roller_voltage;
break;
case RotationState::ROTATING_RIGHT:
- output->left_intake.voltage_rollers = roller_voltage;
- if (position->right_intake.beam_break) {
- output->right_intake.voltage_rollers = -roller_voltage * 0.9;
+ left_intake_output.voltage_rollers = roller_voltage;
+ if (position->right_intake()->beam_break()) {
+ right_intake_output.voltage_rollers = -roller_voltage * 0.9;
} else {
- output->right_intake.voltage_rollers = -roller_voltage * 0.6;
+ right_intake_output.voltage_rollers = -roller_voltage * 0.6;
}
break;
}
@@ -260,29 +278,31 @@
rotation_count_ = 0;
stuck_count_ = 0;
}
- status->rotation_state = static_cast<uint32_t>(rotation_state_);
+ status_builder.add_rotation_state(static_cast<uint32_t>(rotation_state_));
drivetrain_output_fetcher_.Fetch();
vision_status_fetcher_.Fetch();
- if (status->estopped) {
+ if (estopped) {
SendColors(0.5, 0.0, 0.0);
} else if (!vision_status_fetcher_.get() ||
monotonic_now >
- vision_status_fetcher_->sent_time + chrono::seconds(1)) {
+ vision_status_fetcher_.context().monotonic_sent_time +
+ chrono::seconds(1)) {
SendColors(0.5, 0.5, 0.0);
} else if (rotation_state_ == RotationState::ROTATING_LEFT ||
rotation_state_ == RotationState::ROTATING_RIGHT) {
SendColors(0.5, 0.20, 0.0);
} else if (rotation_state_ == RotationState::STUCK) {
SendColors(0.5, 0.0, 0.5);
- } else if (position->box_back_beambreak_triggered) {
+ } else if (position->box_back_beambreak_triggered()) {
SendColors(0.0, 0.0, 0.5);
- } else if (position->box_distance < 0.2) {
+ } else if (position->box_distance() < 0.2) {
SendColors(0.0, 0.5, 0.0);
} else if (drivetrain_output_fetcher_.get() &&
- ::std::max(::std::abs(drivetrain_output_fetcher_->left_voltage),
- ::std::abs(drivetrain_output_fetcher_->right_voltage)) >
+ ::std::max(
+ ::std::abs(drivetrain_output_fetcher_->left_voltage()),
+ ::std::abs(drivetrain_output_fetcher_->right_voltage())) >
11.5) {
SendColors(0.5, 0.0, 0.5);
} else {
@@ -290,15 +310,41 @@
}
last_box_distance_ = clipped_box_distance;
+
+ if (output) {
+ flatbuffers::Offset<IntakeVoltage> left_intake_offset =
+ IntakeVoltage::Pack(*output->fbb(), &left_intake_output);
+ flatbuffers::Offset<IntakeVoltage> right_intake_offset =
+ IntakeVoltage::Pack(*output->fbb(), &right_intake_output);
+
+ Output::Builder output_builder = output->MakeBuilder<Output>();
+ output_builder.add_left_intake(left_intake_offset);
+ output_builder.add_right_intake(right_intake_offset);
+ output_builder.add_voltage_proximal(voltage_proximal_output);
+ output_builder.add_voltage_distal(voltage_distal_output);
+ output_builder.add_release_arm_brake(release_arm_brake_output);
+ output_builder.add_claw_grabbed(claw_grabbed_output);
+
+ output_builder.add_hook_release(hook_release_output);
+ output_builder.add_forks_release(forks_release_output);
+ output_builder.add_voltage_winch(voltage_winch_output);
+
+ output->Send(output_builder.Finish());
+ }
+
+ status->Send(status_builder.Finish());
}
void Superstructure::SendColors(float red, float green, float blue) {
- auto new_status_light = status_light_sender_.MakeMessage();
- new_status_light->red = red;
- new_status_light->green = green;
- new_status_light->blue = blue;
+ auto builder = status_light_sender_.MakeBuilder();
+ StatusLight::Builder status_light_builder =
+ builder.MakeBuilder<StatusLight>();
- if (!new_status_light.Send()) {
+ status_light_builder.add_red(red);
+ status_light_builder.add_green(green);
+ status_light_builder.add_blue(blue);
+
+ if (!builder.Send(status_light_builder.Finish())) {
AOS_LOG(ERROR, "Failed to send lights.\n");
}
}
diff --git a/y2018/control_loops/superstructure/superstructure.h b/y2018/control_loops/superstructure/superstructure.h
index 81b6d9d..78851df 100644
--- a/y2018/control_loops/superstructure/superstructure.h
+++ b/y2018/control_loops/superstructure/superstructure.h
@@ -4,36 +4,37 @@
#include <memory>
#include "aos/controls/control_loop.h"
-#include "aos/events/event-loop.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "aos/events/event_loop.h"
+#include "aos/time/time.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "y2018/control_loops/superstructure/arm/arm.h"
#include "y2018/control_loops/superstructure/intake/intake.h"
-#include "y2018/control_loops/superstructure/superstructure.q.h"
-#include "y2018/status_light.q.h"
-#include "y2018/vision/vision.q.h"
+#include "y2018/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2018/control_loops/superstructure/superstructure_output_generated.h"
+#include "y2018/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2018/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2018/status_light_generated.h"
+#include "y2018/vision/vision_generated.h"
namespace y2018 {
namespace control_loops {
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 = ".y2018.control_loops.superstructure_queue");
+ explicit Superstructure(::aos::EventLoop *event_loop,
+ const ::std::string &name = "/superstructure");
const intake::IntakeSide &intake_left() const { return intake_left_; }
const intake::IntakeSide &intake_right() const { return intake_right_; }
const arm::Arm &arm() const { return arm_; }
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:
// Sends the status light message for the 3 colors provided.
@@ -41,7 +42,7 @@
::aos::Sender<::y2018::StatusLight> status_light_sender_;
::aos::Fetcher<::y2018::vision::VisionStatus> vision_status_fetcher_;
- ::aos::Fetcher<::frc971::control_loops::DrivetrainQueue::Output>
+ ::aos::Fetcher<::frc971::control_loops::drivetrain::Output>
drivetrain_output_fetcher_;
intake::IntakeSide intake_left_;
diff --git a/y2018/control_loops/superstructure/superstructure.q b/y2018/control_loops/superstructure/superstructure.q
deleted file mode 100644
index 049a406..0000000
--- a/y2018/control_loops/superstructure/superstructure.q
+++ /dev/null
@@ -1,222 +0,0 @@
-package y2018.control_loops;
-
-import "aos/controls/control_loops.q";
-import "frc971/control_loops/control_loops.q";
-
-struct IntakeSideStatus {
- // Is the subsystem zeroed?
- bool zeroed;
-
- // The state of the subsystem, if applicable.
- int32_t state;
-
- // If true, we have aborted.
- bool estopped;
-
- // Estimated position of the spring.
- float spring_position;
- // Estimated velocity of the spring in units/second.
- float spring_velocity;
-
- // Estimated position of the joint.
- float motor_position;
- // Estimated velocity of the joint in units/second.
- float motor_velocity;
-
- // Goal position of the joint.
- float goal_position;
- // Goal velocity of the joint in units/second.
- float goal_velocity;
-
- // The calculated velocity with delta x/delta t
- float calculated_velocity;
-
- // The voltage given last cycle;
- float delayed_voltage;
-
- // State of the estimator.
- .frc971.PotAndAbsoluteEncoderEstimatorState estimator_state;
-};
-
-struct IntakeGoal {
- double roller_voltage;
-
- // Goal angle in radians of the intake.
- // Zero radians is where the intake is pointing straight out, with positive
- // radians inward towards the cube.
- double left_intake_angle;
- double right_intake_angle;
-};
-
-struct IntakeElasticSensors {
- // Position of the motor end of the series elastic in radians.
- .frc971.PotAndAbsolutePosition motor_position;
-
- // Displacement of the spring in radians.
- double spring_angle;
-
- // False if the beam break sensor isn't triggered, true if the beam breaker is
- // triggered.
- bool beam_break;
-};
-
-struct ArmStatus {
- // State of the estimators.
- .frc971.PotAndAbsoluteEncoderEstimatorState proximal_estimator_state;
- .frc971.PotAndAbsoluteEncoderEstimatorState distal_estimator_state;
-
- // The node we are currently going to.
- uint32_t current_node;
- // Distance (in radians) to the end of the path.
- float path_distance_to_go;
- // Goal position and velocity (radians)
- float goal_theta0;
- float goal_theta1;
- float goal_omega0;
- float goal_omega1;
-
- // Current position and velocity (radians)
- float theta0;
- float theta1;
-
- float omega0;
- float omega1;
-
- // Estimated voltage error for the two joints.
- float voltage_error0;
- float voltage_error1;
-
- // True if we are zeroed.
- bool zeroed;
-
- // True if the arm is zeroed.
- bool estopped;
-
- // The current state machine state.
- uint32_t state;
-
- uint32_t grab_state;
-
- // The number of times the LQR solver failed.
- uint32_t failed_solutions;
-};
-
-struct ArmPosition {
- // Values of the encoder and potentiometer at the base of the proximal
- // (connected to drivebase) arm in radians.
- .frc971.PotAndAbsolutePosition proximal;
-
- // Values of the encoder and potentiometer at the base of the distal
- // (connected to proximal) arm in radians.
- .frc971.PotAndAbsolutePosition distal;
-};
-
-struct IntakeVoltage {
- // Voltage of the motors on the series elastic on one side (left or right) of
- // the intake.
- double voltage_elastic;
-
- // Voltage of the rollers on one side (left or right) of the intake.
- double voltage_rollers;
-};
-
-// Published on ".y2018.control_loops.superstructure_queue"
-queue_group SuperstructureQueue {
- implements aos.control_loops.ControlLoop;
-
- message Goal {
- IntakeGoal intake;
-
- // Used to identiy a position in the planned set of positions on the arm.
- uint32_t arm_goal_position;
- // If true, start the grab box sequence.
- bool grab_box;
-
- bool open_claw;
- bool close_claw;
-
- bool deploy_fork;
-
- bool hook_release;
-
- double voltage_winch;
-
- double open_threshold;
-
- bool disable_box_correct;
-
- bool trajectory_override;
- };
-
- message Status {
- // Are all the subsystems zeroed?
- bool zeroed;
-
- // If true, any of the subsystems have aborted.
- bool estopped;
-
- // Status of both intake sides.
- IntakeSideStatus left_intake;
- IntakeSideStatus right_intake;
-
- ArmStatus arm;
-
- double filtered_box_velocity;
- uint32_t rotation_state;
- };
-
- message Position {
- // Values of the series elastic encoders on the left side of the robot from
- // the rear perspective in radians.
- IntakeElasticSensors left_intake;
-
- // Values of the series elastic encoders on the right side of the robot from
- // the rear perspective in radians.
- IntakeElasticSensors right_intake;
-
- ArmPosition arm;
-
- // Value of the beam breaker sensor. This value is true if the beam is
- // broken, false if the beam isn't broken.
- bool claw_beambreak_triggered;
- // Value of the beambreak sensor detecting when the box has hit the frame
- // cutout.
- bool box_back_beambreak_triggered;
-
- // Distance to the box in meters.
- double box_distance;
- };
-
- message Output {
- // Voltage sent to the parts on the left side of the intake.
- IntakeVoltage left_intake;
-
- // Voltage sent to the parts on the right side of the intake.
- IntakeVoltage right_intake;
-
- // Voltage sent to the motors on the proximal joint of the arm.
- double voltage_proximal;
-
- // Voltage sent to the motors on the distal joint of the arm.
- double voltage_distal;
-
- // Voltage sent to the hanger. Positive pulls the robot up.
- double voltage_winch;
-
- // Clamped (when true) or unclamped (when false) status sent to the
- // pneumatic claw on the arm.
- bool claw_grabbed;
-
- // If true, release the arm brakes.
- bool release_arm_brake;
- // If true, release the hook
- bool hook_release;
- // If true, release the forks
- bool forks_release;
- };
-
- queue Goal goal;
- queue Output output;
- queue Status status;
- queue Position position;
-};
diff --git a/y2018/control_loops/superstructure/superstructure_goal.fbs b/y2018/control_loops/superstructure/superstructure_goal.fbs
new file mode 100644
index 0000000..b618c1b
--- /dev/null
+++ b/y2018/control_loops/superstructure/superstructure_goal.fbs
@@ -0,0 +1,39 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2018.control_loops.superstructure;
+
+table IntakeGoal {
+ roller_voltage:double;
+
+ // Goal angle in radians of the intake.
+ // Zero radians is where the intake is pointing straight out, with positive
+ // radians inward towards the cube.
+ left_intake_angle:double;
+ right_intake_angle:double;
+}
+
+table Goal {
+ intake:IntakeGoal;
+
+ // Used to identiy a position in the planned set of positions on the arm.
+ arm_goal_position:uint;
+ // If true, start the grab box sequence.
+ grab_box:bool;
+
+ open_claw:bool;
+ close_claw:bool;
+
+ deploy_fork:bool;
+
+ hook_release:bool;
+
+ voltage_winch:double;
+
+ open_threshold:double;
+
+ disable_box_correct:bool;
+
+ trajectory_override:bool;
+}
+
+root_type Goal;
diff --git a/y2018/control_loops/superstructure/superstructure_lib_test.cc b/y2018/control_loops/superstructure/superstructure_lib_test.cc
index 162db33..988655f 100644
--- a/y2018/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2018/control_loops/superstructure/superstructure_lib_test.cc
@@ -6,8 +6,6 @@
#include <memory>
#include "aos/controls/control_loop_test.h"
-#include "aos/queue.h"
-#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
@@ -15,8 +13,8 @@
#include "y2018/control_loops/superstructure/arm/dynamics.h"
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
#include "y2018/control_loops/superstructure/intake/intake_plant.h"
-#include "y2018/status_light.q.h"
-#include "y2018/vision/vision.q.h"
+#include "y2018/status_light_generated.h"
+#include "y2018/vision/vision_generated.h"
using ::frc971::control_loops::PositionSensorSimulator;
@@ -71,9 +69,18 @@
zeroing_constants_.measured_absolute_position);
}
- void GetSensorValues(IntakeElasticSensors *position) {
- pot_encoder_.GetSensorValues(&position->motor_position);
- position->spring_angle = plant_.Y(0);
+ flatbuffers::Offset<IntakeElasticSensors> GetSensorValues(
+ flatbuffers::FlatBufferBuilder *fbb) {
+ frc971::PotAndAbsolutePosition::Builder motor_position_builder(*fbb);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> motor_position_offset =
+ pot_encoder_.GetSensorValues(&motor_position_builder);
+
+ IntakeElasticSensors::Builder intake_elastic_sensors_builder(*fbb);
+
+ intake_elastic_sensors_builder.add_motor_position(motor_position_offset);
+ intake_elastic_sensors_builder.add_spring_angle(plant_.Y(0));
+
+ return intake_elastic_sensors_builder.Finish();
}
double spring_position() const { return plant_.X(0); }
@@ -85,14 +92,14 @@
plant_.set_voltage_offset(voltage_offset);
}
- void Simulate(const IntakeVoltage &intake_voltage) {
+ void Simulate(const IntakeVoltage *intake_voltage) {
const double voltage_check =
superstructure::intake::IntakeSide::kOperatingVoltage();
- AOS_CHECK_LE(::std::abs(intake_voltage.voltage_elastic), voltage_check);
+ AOS_CHECK_LE(::std::abs(intake_voltage->voltage_elastic()), voltage_check);
::Eigen::Matrix<double, 1, 1> U;
- U << intake_voltage.voltage_elastic + plant_.voltage_offset();
+ U << intake_voltage->voltage_elastic() + plant_.voltage_offset();
plant_.Update(U);
@@ -141,9 +148,21 @@
distal_zeroing_constants_.measured_absolute_position);
}
- void GetSensorValues(ArmPosition *position) {
- proximal_pot_encoder_.GetSensorValues(&position->proximal);
- distal_pot_encoder_.GetSensorValues(&position->distal);
+ flatbuffers::Offset<ArmPosition> GetSensorValues(
+ flatbuffers::FlatBufferBuilder *fbb) {
+ frc971::PotAndAbsolutePosition::Builder proximal_builder(*fbb);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> proximal_offset =
+ proximal_pot_encoder_.GetSensorValues(&proximal_builder);
+
+ frc971::PotAndAbsolutePosition::Builder distal_builder(*fbb);
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> distal_offset =
+ distal_pot_encoder_.GetSensorValues(&distal_builder);
+
+ ArmPosition::Builder arm_position_builder(*fbb);
+ arm_position_builder.add_proximal(proximal_offset);
+ arm_position_builder.add_distal(distal_offset);
+
+ return arm_position_builder.Finish();
}
double proximal_position() const { return X_(0, 0); }
@@ -184,7 +203,6 @@
PositionSensorSimulator distal_pot_encoder_;
};
-
class SuperstructureSimulation {
public:
SuperstructureSimulation(::aos::EventLoop *event_loop)
@@ -198,14 +216,14 @@
arm_(constants::GetValues().arm_proximal.zeroing,
constants::GetValues().arm_distal.zeroing),
superstructure_position_sender_(
- event_loop_->MakeSender<SuperstructureQueue::Position>(
- ".y2018.control_loops.superstructure.position")),
+ event_loop_->MakeSender<superstructure::Position>(
+ "/superstructure")),
superstructure_status_fetcher_(
- event_loop_->MakeFetcher<SuperstructureQueue::Status>(
- ".y2018.control_loops.superstructure.status")),
+ event_loop_->MakeFetcher<superstructure::Status>(
+ "/superstructure")),
superstructure_output_fetcher_(
- event_loop_->MakeFetcher<SuperstructureQueue::Output>(
- ".y2018.control_loops.superstructure.output")) {
+ event_loop_->MakeFetcher<superstructure::Output>(
+ "/superstructure")) {
// Start the intake out in the middle by default.
InitializeIntakePosition((constants::Values::kIntakeRange().lower +
constants::Values::kIntakeRange().upper) /
@@ -235,13 +253,20 @@
}
void SendPositionMessage() {
- auto position = superstructure_position_sender_.MakeMessage();
+ auto builder = superstructure_position_sender_.MakeBuilder();
- left_intake_.GetSensorValues(&position->left_intake);
- right_intake_.GetSensorValues(&position->right_intake);
- arm_.GetSensorValues(&position->arm);
- AOS_LOG_STRUCT(INFO, "sim position", *position);
- position.Send();
+ flatbuffers::Offset<IntakeElasticSensors> left_intake_offset =
+ left_intake_.GetSensorValues(builder.fbb());
+ flatbuffers::Offset<IntakeElasticSensors> right_intake_offset =
+ right_intake_.GetSensorValues(builder.fbb());
+ flatbuffers::Offset<ArmPosition> arm_offset =
+ arm_.GetSensorValues(builder.fbb());
+
+ Position::Builder position_builder = builder.MakeBuilder<Position>();
+ position_builder.add_left_intake(left_intake_offset);
+ position_builder.add_right_intake(right_intake_offset);
+ position_builder.add_arm(arm_offset);
+ EXPECT_TRUE(builder.Send(position_builder.Finish()));
}
// Sets the difference between the commanded and applied powers.
@@ -263,13 +288,13 @@
ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
- left_intake_.Simulate(superstructure_output_fetcher_->left_intake);
- right_intake_.Simulate(superstructure_output_fetcher_->right_intake);
+ left_intake_.Simulate(superstructure_output_fetcher_->left_intake());
+ right_intake_.Simulate(superstructure_output_fetcher_->right_intake());
arm_.Simulate((::Eigen::Matrix<double, 2, 1>()
- << superstructure_output_fetcher_->voltage_proximal,
- superstructure_output_fetcher_->voltage_distal)
+ << superstructure_output_fetcher_->voltage_proximal(),
+ superstructure_output_fetcher_->voltage_distal())
.finished(),
- superstructure_output_fetcher_->release_arm_brake);
+ superstructure_output_fetcher_->release_arm_brake());
}
private:
@@ -280,9 +305,9 @@
IntakeSideSimulation right_intake_;
ArmSimulation arm_;
- ::aos::Sender<SuperstructureQueue::Position> superstructure_position_sender_;
- ::aos::Fetcher<SuperstructureQueue::Status> superstructure_status_fetcher_;
- ::aos::Fetcher<SuperstructureQueue::Output> superstructure_output_fetcher_;
+ ::aos::Sender<superstructure::Position> superstructure_position_sender_;
+ ::aos::Fetcher<superstructure::Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<superstructure::Output> superstructure_output_fetcher_;
bool first_ = true;
};
@@ -290,23 +315,24 @@
class SuperstructureTest : public ::aos::testing::ControlLoopTest {
protected:
SuperstructureTest()
- : ::aos::testing::ControlLoopTest(::std::chrono::microseconds(5050)),
+ : ::aos::testing::ControlLoopTest(
+ aos::configuration::ReadConfig("y2018/config.json"),
+ ::std::chrono::microseconds(5050)),
test_event_loop_(MakeEventLoop()),
superstructure_goal_fetcher_(
- test_event_loop_->MakeFetcher<SuperstructureQueue::Goal>(
- ".y2018.control_loops.superstructure.goal")),
+ test_event_loop_->MakeFetcher<superstructure::Goal>(
+ "/superstructure")),
superstructure_goal_sender_(
- test_event_loop_->MakeSender<SuperstructureQueue::Goal>(
- ".y2018.control_loops.superstructure.goal")),
+ test_event_loop_->MakeSender<superstructure::Goal>(
+ "/superstructure")),
superstructure_status_fetcher_(
- test_event_loop_->MakeFetcher<SuperstructureQueue::Status>(
- ".y2018.control_loops.superstructure.status")),
+ test_event_loop_->MakeFetcher<superstructure::Status>(
+ "/superstructure")),
superstructure_output_fetcher_(
- test_event_loop_->MakeFetcher<SuperstructureQueue::Output>(
- ".y2018.control_loops.superstructure.output")),
+ test_event_loop_->MakeFetcher<superstructure::Output>(
+ "/superstructure")),
superstructure_event_loop_(MakeEventLoop()),
- superstructure_(superstructure_event_loop_.get(),
- ".y2018.control_loops.superstructure"),
+ superstructure_(superstructure_event_loop_.get(), "/superstructure"),
superstructure_plant_event_loop_(MakeEventLoop()),
superstructure_plant_(superstructure_plant_event_loop_.get()) {
set_team_id(::frc971::control_loops::testing::kTeamNumber);
@@ -319,28 +345,30 @@
ASSERT_TRUE(superstructure_goal_fetcher_.get() != nullptr) << ": No goal";
ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
// Left side test.
- EXPECT_NEAR(superstructure_goal_fetcher_->intake.left_intake_angle,
- superstructure_status_fetcher_->left_intake.spring_position +
- superstructure_status_fetcher_->left_intake.motor_position,
- 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->intake.left_intake_angle,
+ EXPECT_NEAR(
+ superstructure_goal_fetcher_->intake()->left_intake_angle(),
+ superstructure_status_fetcher_->left_intake()->spring_position() +
+ superstructure_status_fetcher_->left_intake()->motor_position(),
+ 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->intake()->left_intake_angle(),
superstructure_plant_.left_intake().spring_position(), 0.001);
// Right side test.
- EXPECT_NEAR(superstructure_goal_fetcher_->intake.right_intake_angle,
- superstructure_status_fetcher_->right_intake.spring_position +
- superstructure_status_fetcher_->right_intake.motor_position,
- 0.001);
- EXPECT_NEAR(superstructure_goal_fetcher_->intake.right_intake_angle,
+ EXPECT_NEAR(
+ superstructure_goal_fetcher_->intake()->right_intake_angle(),
+ superstructure_status_fetcher_->right_intake()->spring_position() +
+ superstructure_status_fetcher_->right_intake()->motor_position(),
+ 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->intake()->right_intake_angle(),
superstructure_plant_.right_intake().spring_position(), 0.001);
}
::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<superstructure::Goal> superstructure_goal_fetcher_;
+ ::aos::Sender<superstructure::Goal> superstructure_goal_sender_;
+ ::aos::Fetcher<superstructure::Status> superstructure_status_fetcher_;
+ ::aos::Fetcher<superstructure::Output> superstructure_output_fetcher_;
// Create a control loop and simulation.
::std::unique_ptr<::aos::EventLoop> superstructure_event_loop_;
@@ -360,8 +388,10 @@
superstructure_.intake_left().state());
EXPECT_EQ(intake::IntakeSide::State::RUNNING,
superstructure_.intake_right().state());
- EXPECT_EQ(superstructure_output_fetcher_->left_intake.voltage_elastic, 0.0);
- EXPECT_EQ(superstructure_output_fetcher_->right_intake.voltage_elastic, 0.0);
+ EXPECT_EQ(superstructure_output_fetcher_->left_intake()->voltage_elastic(),
+ 0.0);
+ EXPECT_EQ(superstructure_output_fetcher_->right_intake()->voltage_elastic(),
+ 0.0);
}
// Tests that the intake loop can reach a goal.
@@ -369,14 +399,20 @@
SetEnabled(true);
// Set a reasonable goal.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->intake.left_intake_angle = 0.1;
- goal->intake.right_intake_angle = 0.2;
- goal->arm_goal_position = arm::UpIndex();
- goal->open_claw = true;
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(0.1);
+ intake_goal_builder.add_right_intake_angle(0.2);
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::UpIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Give it a lot of time to get there.
@@ -393,14 +429,20 @@
// Set a reasonable goal.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->intake.left_intake_angle = 0.1;
- goal->intake.right_intake_angle = 0.2;
- goal->arm_goal_position = arm::UpIndex();
- goal->open_claw = true;
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(0.1);
+ intake_goal_builder.add_right_intake_angle(0.2);
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::UpIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
// Give it a lot of time to get there.
@@ -415,43 +457,58 @@
SetEnabled(true);
// Set some ridiculous goals to test upper limits.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->intake.left_intake_angle = 5.0 * M_PI;
- goal->intake.right_intake_angle = 5.0 * M_PI;
- goal->arm_goal_position = arm::UpIndex();
- goal->open_claw = true;
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(5.0 * M_PI);
+ intake_goal_builder.add_right_intake_angle(5.0 * M_PI);
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::UpIndex());
+ goal_builder.add_open_claw(true);
+
+ 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(0.0, superstructure_status_fetcher_->left_intake.spring_position,
+ EXPECT_NEAR(0.0,
+ superstructure_status_fetcher_->left_intake()->spring_position(),
0.001);
- EXPECT_NEAR(constants::Values::kIntakeRange().upper,
- superstructure_status_fetcher_->left_intake.spring_position +
- superstructure_status_fetcher_->left_intake.motor_position,
- 0.001);
+ EXPECT_NEAR(
+ constants::Values::kIntakeRange().upper,
+ superstructure_status_fetcher_->left_intake()->spring_position() +
+ superstructure_status_fetcher_->left_intake()->motor_position(),
+ 0.001);
- EXPECT_NEAR(0.0, superstructure_status_fetcher_->right_intake.spring_position,
+ EXPECT_NEAR(0.0,
+ superstructure_status_fetcher_->right_intake()->spring_position(),
0.001);
EXPECT_NEAR(constants::Values::kIntakeRange().upper,
- superstructure_status_fetcher_->right_intake.motor_position,
+ superstructure_status_fetcher_->right_intake()->motor_position(),
0.001);
// Set some ridiculous goals to test lower limits.
{
- auto goal = superstructure_goal_sender_.MakeMessage();
+ auto builder = superstructure_goal_sender_.MakeBuilder();
- goal->intake.left_intake_angle = -5.0 * M_PI;
- goal->intake.right_intake_angle = -5.0 * M_PI;
- goal->arm_goal_position = arm::UpIndex();
- goal->open_claw = true;
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(-5.0 * M_PI);
+ intake_goal_builder.add_right_intake_angle(-5.0 * M_PI);
- ASSERT_TRUE(goal.Send());
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::UpIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -460,13 +517,17 @@
superstructure_status_fetcher_.Fetch();
EXPECT_NEAR(constants::Values::kIntakeRange().lower,
- superstructure_status_fetcher_->left_intake.motor_position, 0.001);
- EXPECT_NEAR(0.0, superstructure_status_fetcher_->left_intake.spring_position,
+ superstructure_status_fetcher_->left_intake()->motor_position(),
+ 0.001);
+ EXPECT_NEAR(0.0,
+ superstructure_status_fetcher_->left_intake()->spring_position(),
0.001);
EXPECT_NEAR(constants::Values::kIntakeRange().lower,
- superstructure_status_fetcher_->right_intake.motor_position, 0.001);
- EXPECT_NEAR(0.0, superstructure_status_fetcher_->right_intake.spring_position,
+ superstructure_status_fetcher_->right_intake()->motor_position(),
+ 0.001);
+ EXPECT_NEAR(0.0,
+ superstructure_status_fetcher_->right_intake()->spring_position(),
0.001);
}
@@ -475,19 +536,40 @@
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange().lower_hard);
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.left_intake_angle = constants::Values::kIntakeRange().lower;
- goal->intake.right_intake_angle = constants::Values::kIntakeRange().lower;
- goal->arm_goal_position = arm::UpIndex();
- goal->open_claw = true;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(
+ constants::Values::kIntakeRange().lower);
+ intake_goal_builder.add_right_intake_angle(
+ constants::Values::kIntakeRange().lower);
+
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::UpIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.left_intake_angle = 1.0;
- goal->intake.right_intake_angle = 1.0;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(1.0);
+ intake_goal_builder.add_right_intake_angle(1.0);
+
+ flatbuffers::Offset<IntakeGoal> intake_offset =
+ intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::UpIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
VerifyNearGoal();
@@ -499,12 +581,22 @@
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange().upper_hard);
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.left_intake_angle = constants::Values::kIntakeRange().upper;
- goal->intake.right_intake_angle = constants::Values::kIntakeRange().upper;
- goal->arm_goal_position = arm::UpIndex();
- goal->open_claw = true;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(
+ constants::Values::kIntakeRange().upper);
+ intake_goal_builder.add_right_intake_angle(
+ constants::Values::kIntakeRange().upper);
+
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::UpIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -517,14 +609,22 @@
superstructure_plant_.InitializeIntakePosition(
constants::Values::kIntakeRange().upper);
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.left_intake_angle =
- constants::Values::kIntakeRange().upper - 0.1;
- goal->intake.right_intake_angle =
- constants::Values::kIntakeRange().upper - 0.1;
- goal->arm_goal_position = arm::UpIndex();
- goal->open_claw = true;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(
+ constants::Values::kIntakeRange().upper - 0.1);
+ intake_goal_builder.add_right_intake_angle(
+ constants::Values::kIntakeRange().upper - 0.1);
+
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::UpIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -558,12 +658,20 @@
RunFor(chrono::seconds(5));
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.left_intake_angle = -0.8;
- goal->intake.right_intake_angle = -0.8;
- goal->arm_goal_position = arm::FrontHighBoxIndex();
- goal->open_claw = true;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(-0.8);
+ intake_goal_builder.add_right_intake_angle(-0.8);
+
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::FrontHighBoxIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
EXPECT_EQ(arm::Arm::State::RUNNING, superstructure_.arm().state());
@@ -573,12 +681,20 @@
TEST_F(SuperstructureTest, ArmMoveAndMoveBack) {
SetEnabled(true);
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.left_intake_angle = 0.0;
- goal->intake.right_intake_angle = 0.0;
- goal->arm_goal_position = arm::FrontHighBoxIndex();
- goal->open_claw = true;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(0.0);
+ intake_goal_builder.add_right_intake_angle(0.0);
+
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::FrontHighBoxIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -586,12 +702,20 @@
VerifyNearGoal();
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.left_intake_angle = 0.0;
- goal->intake.right_intake_angle = 0.0;
- goal->arm_goal_position = arm::ReadyAboveBoxIndex();
- goal->open_claw = true;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(0.0);
+ intake_goal_builder.add_right_intake_angle(0.0);
+
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::ReadyAboveBoxIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -604,12 +728,20 @@
superstructure_plant_.InitializeArmPosition(arm::ReadyAboveBoxPoint());
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.left_intake_angle = 0.0;
- goal->intake.right_intake_angle = 0.0;
- goal->arm_goal_position = arm::BackLowBoxIndex();
- goal->open_claw = true;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(0.0);
+ intake_goal_builder.add_right_intake_angle(0.0);
+
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::BackLowBoxIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
@@ -617,12 +749,20 @@
VerifyNearGoal();
{
- auto goal = superstructure_goal_sender_.MakeMessage();
- goal->intake.left_intake_angle = 0.0;
- goal->intake.right_intake_angle = 0.0;
- goal->arm_goal_position = arm::ReadyAboveBoxIndex();
- goal->open_claw = true;
- ASSERT_TRUE(goal.Send());
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ IntakeGoal::Builder intake_goal_builder = builder.MakeBuilder<IntakeGoal>();
+ intake_goal_builder.add_left_intake_angle(0.0);
+ intake_goal_builder.add_right_intake_angle(0.0);
+
+ flatbuffers::Offset<IntakeGoal> intake_offset = intake_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+ goal_builder.add_intake(intake_offset);
+ goal_builder.add_arm_goal_position(arm::ReadyAboveBoxIndex());
+ goal_builder.add_open_claw(true);
+
+ ASSERT_TRUE(builder.Send(goal_builder.Finish()));
}
RunFor(chrono::seconds(10));
diff --git a/y2018/control_loops/superstructure/superstructure_main.cc b/y2018/control_loops/superstructure/superstructure_main.cc
index 789f13f..3200ead 100644
--- a/y2018/control_loops/superstructure/superstructure_main.cc
+++ b/y2018/control_loops/superstructure/superstructure_main.cc
@@ -1,12 +1,15 @@
#include "y2018/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());
::y2018::control_loops::superstructure::Superstructure superstructure(
&event_loop);
diff --git a/y2018/control_loops/superstructure/superstructure_output.fbs b/y2018/control_loops/superstructure/superstructure_output.fbs
new file mode 100644
index 0000000..f4d62da
--- /dev/null
+++ b/y2018/control_loops/superstructure/superstructure_output.fbs
@@ -0,0 +1,40 @@
+namespace y2018.control_loops.superstructure;
+
+table IntakeVoltage {
+ // Voltage of the motors on the series elastic on one side (left or right) of
+ // the intake.
+ voltage_elastic:double;
+
+ // Voltage of the rollers on one side (left or right) of the intake.
+ voltage_rollers:double;
+}
+
+table Output {
+ // Voltage sent to the parts on the left side of the intake.
+ left_intake:IntakeVoltage;
+
+ // Voltage sent to the parts on the right side of the intake.
+ right_intake:IntakeVoltage;
+
+ // Voltage sent to the motors on the proximal joint of the arm.
+ voltage_proximal:double;
+
+ // Voltage sent to the motors on the distal joint of the arm.
+ voltage_distal:double;
+
+ // Voltage sent to the hanger. Positive pulls the robot up.
+ voltage_winch:double;
+
+ // Clamped (when true) or unclamped (when false) status sent to the
+ // pneumatic claw on the arm.
+ claw_grabbed:bool;
+
+ // If true, release the arm brakes.
+ release_arm_brake:bool;
+ // If true, release the hook
+ hook_release:bool;
+ // If true, release the forks
+ forks_release:bool;
+}
+
+root_type Output;
diff --git a/y2018/control_loops/superstructure/superstructure_position.fbs b/y2018/control_loops/superstructure/superstructure_position.fbs
new file mode 100644
index 0000000..0323b78
--- /dev/null
+++ b/y2018/control_loops/superstructure/superstructure_position.fbs
@@ -0,0 +1,50 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2018.control_loops.superstructure;
+
+table IntakeElasticSensors {
+ // Position of the motor end of the series elastic in radians.
+ motor_position:frc971.PotAndAbsolutePosition;
+
+ // Displacement of the spring in radians.
+ spring_angle:double;
+
+ // False if the beam break sensor isn't triggered, true if the beam breaker is
+ // triggered.
+ beam_break:bool;
+}
+
+table ArmPosition {
+ // Values of the encoder and potentiometer at the base of the proximal
+ // (connected to drivebase) arm in radians.
+ proximal:frc971.PotAndAbsolutePosition;
+
+ // Values of the encoder and potentiometer at the base of the distal
+ // (connected to proximal) arm in radians.
+ distal:frc971.PotAndAbsolutePosition;
+}
+
+
+table Position {
+ // Values of the series elastic encoders on the left side of the robot from
+ // the rear perspective in radians.
+ left_intake:IntakeElasticSensors;
+
+ // Values of the series elastic encoders on the right side of the robot from
+ // the rear perspective in radians.
+ right_intake:IntakeElasticSensors;
+
+ arm:ArmPosition;
+
+ // Value of the beam breaker sensor. This value is true if the beam is
+ // broken, false if the beam isn't broken.
+ claw_beambreak_triggered:bool;
+ // Value of the beambreak sensor detecting when the box has hit the frame
+ // cutout.
+ box_back_beambreak_triggered:bool;
+
+ // Distance to the box in meters.
+ box_distance:double;
+}
+
+root_type Position;
diff --git a/y2018/control_loops/superstructure/superstructure_status.fbs b/y2018/control_loops/superstructure/superstructure_status.fbs
new file mode 100644
index 0000000..af2d1ab
--- /dev/null
+++ b/y2018/control_loops/superstructure/superstructure_status.fbs
@@ -0,0 +1,98 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace y2018.control_loops.superstructure;
+
+table IntakeSideStatus {
+ // Is the subsystem zeroed?
+ zeroed:bool;
+
+ // The state of the subsystem, if applicable.
+ state:int;
+
+ // If true, we have aborted.
+ estopped:bool;
+
+ // Estimated position of the spring.
+ spring_position:float;
+ // Estimated velocity of the spring in units/second.
+ spring_velocity:float;
+
+ // Estimated position of the joint.
+ motor_position:float;
+ // Estimated velocity of the joint in units/second.
+ motor_velocity:float;
+
+ // Goal position of the joint.
+ goal_position:float;
+ // Goal velocity of the joint in units/second.
+ goal_velocity:float;
+
+ // The calculated velocity with delta x/delta t
+ calculated_velocity:float;
+
+ // The voltage given last cycle;
+ delayed_voltage:float;
+
+ // State of the estimator.
+ estimator_state:frc971.PotAndAbsoluteEncoderEstimatorState;
+}
+
+table ArmStatus {
+ // State of the estimators.
+ proximal_estimator_state:frc971.PotAndAbsoluteEncoderEstimatorState;
+ distal_estimator_state:frc971.PotAndAbsoluteEncoderEstimatorState;
+
+ // The node we are currently going to.
+ current_node:uint;
+ // Distance (in radians) to the end of the path.
+ path_distance_to_go:float;
+ // Goal position and velocity (radians)
+ goal_theta0:float;
+ goal_theta1:float;
+ goal_omega0:float;
+ goal_omega1:float;
+
+ // Current position and velocity (radians)
+ theta0:float;
+ theta1:float;
+
+ omega0:float;
+ omega1:float;
+
+ // Estimated voltage error for the two joints.
+ voltage_error0:float;
+ voltage_error1:float;
+
+ // True if we are zeroed.
+ zeroed:bool;
+
+ // True if the arm is zeroed.
+ estopped:bool;
+
+ // The current state machine state.
+ state:uint;
+
+ grab_state:uint;
+
+ // The number of times the LQR solver failed.
+ failed_solutions:uint;
+}
+
+table Status {
+ // Are all the subsystems zeroed?
+ zeroed:bool;
+
+ // If true, any of the subsystems have aborted.
+ estopped:bool;
+
+ // Status of both intake sides.
+ left_intake:IntakeSideStatus;
+ right_intake:IntakeSideStatus;
+
+ arm:ArmStatus;
+
+ filtered_box_velocity:double;
+ rotation_state:uint;
+}
+
+root_type Status;