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/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(); }