Added superstructure and intake classes.
Created the superstucture, intake, and intake controller class,
had to alter some of the intake python and superstructure queue to fit.
Change-Id: Ieabcf288f6dd50c282a3bcb61ec13062f735872b
diff --git a/y2018/control_loops/superstructure/BUILD b/y2018/control_loops/superstructure/BUILD
index 275d46a..c8b0b84 100644
--- a/y2018/control_loops/superstructure/BUILD
+++ b/y2018/control_loops/superstructure/BUILD
@@ -1,17 +1,62 @@
-package(default_visibility = ['//visibility:public'])
+package(default_visibility = ["//visibility:public"])
-load('//aos/build:queues.bzl', 'queue_library')
-
+load("//aos/build:queues.bzl", "queue_library")
queue_library(
- name = 'superstructure_queue',
- srcs = [
- 'superstructure.q',
- ],
- deps = [
- '//aos/common/controls:control_loop_queues',
- '//frc971/control_loops:profiled_subsystem_queue',
- '//frc971/control_loops:queues',
- ],
+ name = "superstructure_queue",
+ srcs = [
+ "superstructure.q",
+ ],
+ deps = [
+ "//aos/common/controls:control_loop_queues",
+ "//frc971/control_loops:queues",
+ ],
)
+cc_library(
+ name = "superstructure_lib",
+ srcs = [
+ "superstructure.cc",
+ ],
+ hdrs = [
+ "superstructure.h",
+ ],
+ deps = [
+ ":superstructure_queue",
+ "//aos/common/controls:control_loop",
+ "//frc971/control_loops:queues",
+ "//y2018:constants",
+ "//y2018/control_loops/superstructure/intake",
+ ],
+)
+
+cc_test(
+ name = "superstructure_lib_test",
+ srcs = [
+ "superstructure_lib_test.cc",
+ ],
+ deps = [
+ ":superstructure_lib",
+ ":superstructure_queue",
+ "//aos/common:math",
+ "//aos/common:queues",
+ "//aos/common:time",
+ "//aos/common/controls:control_loop_test",
+ "//aos/testing:googletest",
+ "//frc971/control_loops:position_sensor_sim",
+ "//frc971/control_loops:team_number_test_environment",
+ "//y2018/control_loops/superstructure/intake:intake_plants",
+ ],
+)
+
+cc_binary(
+ name = "superstructure",
+ srcs = [
+ "superstructure_main.cc",
+ ],
+ deps = [
+ ":superstructure_lib",
+ ":superstructure_queue",
+ "//aos/linux_code:init",
+ ],
+)
diff --git a/y2018/control_loops/superstructure/intake/BUILD b/y2018/control_loops/superstructure/intake/BUILD
index bb6fec7..ff2f16e 100644
--- a/y2018/control_loops/superstructure/intake/BUILD
+++ b/y2018/control_loops/superstructure/intake/BUILD
@@ -27,3 +27,24 @@
"//frc971/control_loops:state_feedback_loop",
],
)
+
+cc_library(
+ name = "intake",
+ srcs = [
+ "intake.cc",
+ ],
+ hdrs = [
+ "intake.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":intake_plants",
+ "//aos/common:math",
+ "//aos/common/controls:control_loop",
+ "//aos/common/logging:queue_logging",
+ "//frc971/control_loops:queues",
+ "//frc971/zeroing",
+ "//y2018:constants",
+ "//y2018/control_loops/superstructure:superstructure_queue",
+ ],
+)
diff --git a/y2018/control_loops/superstructure/intake/intake.cc b/y2018/control_loops/superstructure/intake/intake.cc
new file mode 100644
index 0000000..7212fd3
--- /dev/null
+++ b/y2018/control_loops/superstructure/intake/intake.cc
@@ -0,0 +1,172 @@
+#include "y2018/control_loops/superstructure/intake/intake.h"
+
+#include <chrono>
+
+#include "aos/common/commonmath.h"
+#include "aos/common/controls/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
+
+#include "y2018/constants.h"
+#include "y2018/control_loops/superstructure/intake/intake_delayed_plant.h"
+#include "y2018/control_loops/superstructure/intake/intake_plant.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace intake {
+
+namespace chrono = ::std::chrono;
+using ::aos::monotonic_clock;
+
+constexpr double IntakeController::kDt;
+
+IntakeController::IntakeController()
+ : loop_(new StateFeedbackLoop<5, 1, 2, double, StateFeedbackPlant<5, 1, 2>,
+ StateFeedbackObserver<5, 1, 2>>(
+ superstructure::intake::MakeDelayedIntakeLoop())),
+ intake_range_(::y2018::constants::Values::kIntakeRange()) {
+ Y_.setZero();
+}
+
+void IntakeController::set_position(double spring_angle,
+ double output_position) {
+ // Update position in the model.
+ Y_ << spring_angle, (output_position + offset_);
+}
+
+double IntakeController::voltage() const { return loop_->U(0, 0); }
+
+void IntakeController::Reset() { reset_ = true; }
+
+void IntakeController::UpdateOffset(double offset) {
+ const double doffset = offset - offset_;
+ offset_ = offset;
+
+ loop_->mutable_X_hat(0) += doffset;
+ loop_->mutable_X_hat(2) += doffset;
+}
+
+double IntakeController::goal_angle(const double *unsafe_goal) {
+ if (unsafe_goal == nullptr) {
+ return 0;
+ } else {
+ return ::aos::Clip(*unsafe_goal, intake_range_.lower, intake_range_.upper);
+ }
+}
+
+void IntakeController::Update(bool disabled, const double *unsafe_goal) {
+ if (reset_) {
+ loop_->mutable_X_hat().setZero();
+ loop_->mutable_X_hat(0) = Y_(0) + Y_(1);
+ loop_->mutable_X_hat(2) = Y_(1);
+ reset_ = false;
+ }
+
+ double goal_velocity;
+ loop_->Correct(Y_);
+
+ if (unsafe_goal == nullptr) {
+ disabled = true;
+ goal_velocity = 0.0;
+ } else {
+ goal_velocity = ::aos::Clip(
+ ((goal_angle(unsafe_goal) - loop_->X_hat(0, 0)) * 6.0), -10.0, 10.0);
+ }
+ // Computes the goal.
+ loop_->mutable_R() << 0.0, goal_velocity, 0.0, goal_velocity,
+ (goal_velocity / (kGearRatio * kMotorVelocityConstant));
+
+ loop_->Update(disabled);
+}
+
+void IntakeController::SetStatus(IntakeSideStatus *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);
+ status->spring_velocity = loop_->X_hat(1);
+ status->motor_position = loop_->X_hat(2);
+ status->motor_velocity = loop_->X_hat(3);
+ status->delayed_voltage = loop_->X_hat(4);
+}
+
+IntakeSide::IntakeSide(
+ const ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
+ &zeroing_constants)
+ : zeroing_estimator_(zeroing_constants) {}
+
+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) {
+ double intake_last_position_ = status->estimator_state.position;
+ zeroing_estimator_.UpdateEstimate(position->motor_position);
+
+ switch (state_) {
+ case State::UNINITIALIZED:
+ // Wait in the uninitialized state until the intake is initialized.
+ LOG(DEBUG, "Uninitialized, waiting for intake\n");
+ zeroing_estimator_.Reset();
+ controller_.Reset();
+ state_ = State::ZEROING;
+ break;
+
+ case State::ZEROING:
+ // Zero by not moving.
+ if (zeroing_estimator_.zeroed()) {
+ LOG(INFO, "Now zeroed\n");
+ controller_.UpdateOffset(zeroing_estimator_.offset());
+ state_ = State::RUNNING;
+ }
+ break;
+
+ case State::RUNNING:
+ if (!(zeroing_estimator_.zeroed())) {
+ LOG(ERROR, "Zeroing estimator is no longer zeroed\n");
+ state_ = State::UNINITIALIZED;
+ }
+ if (zeroing_estimator_.error()) {
+ LOG(ERROR, "Zeroing estimator error\n");
+ 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) {
+ LOG(ERROR, "Hit hard limits\n");
+ state_ = State::ESTOP;
+ }
+ break;
+
+ case State::ESTOP:
+ LOG(ERROR, "Estop\n");
+ break;
+ }
+
+ const bool disable = (output == nullptr) || state_ != State::RUNNING;
+ controller_.set_position(position->spring_angle,
+ position->motor_position.encoder);
+
+ controller_.Update(disable, unsafe_goal);
+
+ if (output) {
+ output->voltage_elastic = controller_.voltage();
+ }
+
+ // 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_);
+}
+
+} // namespace intake
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2018
diff --git a/y2018/control_loops/superstructure/intake/intake.h b/y2018/control_loops/superstructure/intake/intake.h
new file mode 100644
index 0000000..d46d90e
--- /dev/null
+++ b/y2018/control_loops/superstructure/intake/intake.h
@@ -0,0 +1,107 @@
+#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_INTAKE_INTAKE_H_
+#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_INTAKE_INTAKE_H_
+
+#include "aos/common/commonmath.h"
+#include "aos/common/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"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace intake {
+
+class IntakeController {
+ public:
+ IntakeController();
+
+ // Sets the current encoder position in radians
+ void set_position(double spring_angle, double output_position);
+
+ // Populates the status structure.
+ void SetStatus(control_loops::IntakeSideStatus *status,
+ const double *unsafe_goal);
+
+ // Returns the control loop calculated voltage.
+ double voltage() const;
+
+ // Executes the control loop for a cycle.
+ void Update(bool disabled, const double *unsafe_goal);
+
+ // Resets the kalman filter and any other internal state.
+ void Reset();
+
+ // 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 =
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(
+ ::aos::controls::kLoopFrequency)
+ .count();
+
+ // Sets the offset of the controller to be the zeroing estimator offset when
+ // possible otherwise zero.
+ void UpdateOffset(double offset);
+
+ const ::frc971::constants::Range intake_range_;
+
+ // Stores the current zeroing estimator offset.
+ double offset_ = 0.0;
+
+ private:
+ bool reset_ = true;
+
+ // The current sensor measurement.
+ Eigen::Matrix<double, 2, 1> Y_;
+
+ DISALLOW_COPY_AND_ASSIGN(IntakeController);
+};
+
+class IntakeSide {
+ public:
+ IntakeSide(const ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
+ &zeroing_constants);
+
+ // 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);
+
+ void Reset();
+
+ enum class State : int32_t {
+ UNINITIALIZED,
+ ZEROING,
+ RUNNING,
+ ESTOP,
+ };
+
+ State state() const { return state_; }
+
+ private:
+ IntakeController controller_;
+
+ State state_ = State::UNINITIALIZED;
+
+ ::frc971::zeroing::PotAndAbsEncoderZeroingEstimator zeroing_estimator_;
+};
+
+} // namespace intake
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2018
+
+#endif // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_INTAKE_INTAKE_H_
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
new file mode 100644
index 0000000..157a46e
--- /dev/null
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -0,0 +1,67 @@
+#include "y2018/control_loops/superstructure/superstructure.h"
+
+#include "aos/common/controls/control_loops.q.h"
+#include "aos/common/logging/logging.h"
+#include "frc971/control_loops/control_loops.q.h"
+#include "y2018/constants.h"
+#include "y2018/control_loops/superstructure/intake/intake.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+
+namespace {
+// The maximum voltage the intake roller will be allowed to use.
+constexpr double kMaxIntakeRollerVoltage = 12.0;
+} // namespace
+
+Superstructure::Superstructure(
+ control_loops::SuperstructureQueue *superstructure_queue)
+ : aos::controls::ControlLoop<control_loops::SuperstructureQueue>(
+ superstructure_queue),
+ 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) {
+ if (WasReset()) {
+ LOG(ERROR, "WPILib reset, restarting\n");
+ intake_left_.Reset();
+ intake_right_.Reset();
+ }
+
+ intake_left_.Iterate(unsafe_goal != nullptr
+ ? &(unsafe_goal->intake.left_intake_angle)
+ : nullptr,
+ &(position->intake.left),
+ output != nullptr ? &(output->intake.left) : nullptr,
+ &(status->left_intake));
+
+ intake_right_.Iterate(unsafe_goal != nullptr
+ ? &(unsafe_goal->intake.right_intake_angle)
+ : nullptr,
+ &(position->intake.right),
+ output != nullptr ? &(output->intake.right) : nullptr,
+ &(status->right_intake));
+
+ status->estopped =
+ status->left_intake.estopped || status->right_intake.estopped;
+
+ status->zeroed = status->left_intake.zeroed && status->right_intake.zeroed;
+
+ if (output && unsafe_goal) {
+ output->intake.left.voltage_rollers = ::std::max(
+ -kMaxIntakeRollerVoltage, ::std::min(unsafe_goal->intake.roller_voltage,
+ kMaxIntakeRollerVoltage));
+ output->intake.right.voltage_rollers = ::std::max(
+ -kMaxIntakeRollerVoltage, ::std::min(unsafe_goal->intake.roller_voltage,
+ kMaxIntakeRollerVoltage));
+ }
+}
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2018
diff --git a/y2018/control_loops/superstructure/superstructure.h b/y2018/control_loops/superstructure/superstructure.h
new file mode 100644
index 0000000..0cc504d
--- /dev/null
+++ b/y2018/control_loops/superstructure/superstructure.h
@@ -0,0 +1,43 @@
+#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
+
+#include <memory>
+
+#include "aos/common/controls/control_loop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "y2018/control_loops/superstructure/intake/intake.h"
+#include "y2018/control_loops/superstructure/superstructure.q.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+
+class Superstructure
+ : public ::aos::controls::ControlLoop<control_loops::SuperstructureQueue> {
+ public:
+ explicit Superstructure(
+ control_loops::SuperstructureQueue *my_superstructure =
+ &control_loops::superstructure_queue);
+
+ const intake::IntakeSide &intake_left() const { return intake_left_; }
+ const intake::IntakeSide &intake_right() const { return intake_right_; }
+
+ 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;
+
+ private:
+ intake::IntakeSide intake_left_;
+ intake::IntakeSide intake_right_;
+
+ DISALLOW_COPY_AND_ASSIGN(Superstructure);
+};
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2018
+
+#endif // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_SUPERSTRUCTURE_H_
diff --git a/y2018/control_loops/superstructure/superstructure.q b/y2018/control_loops/superstructure/superstructure.q
index 75900fa..3eaeb7e 100644
--- a/y2018/control_loops/superstructure/superstructure.q
+++ b/y2018/control_loops/superstructure/superstructure.q
@@ -13,10 +13,15 @@
// 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 position;
+ float motor_position;
// Estimated velocity of the joint in units/second.
- float velocity;
+ float motor_velocity;
// Goal position of the joint.
float goal_position;
@@ -26,12 +31,15 @@
// The calculated velocity with delta x/delta t
float calculated_velocity;
+ // The voltage given last cycle;
+ float delayed_voltage;
+
// State of the estimator.
.frc971.AbsoluteEstimatorState estimator_state;
};
struct IntakeGoal {
- float roller_voltage;
+ double roller_voltage;
// Goal angle in radians of the intake.
// Zero radians is where the intake is pointing straight out, with positive
diff --git a/y2018/control_loops/superstructure/superstructure_lib_test.cc b/y2018/control_loops/superstructure/superstructure_lib_test.cc
new file mode 100644
index 0000000..2fa19ca
--- /dev/null
+++ b/y2018/control_loops/superstructure/superstructure_lib_test.cc
@@ -0,0 +1,414 @@
+#include "y2018/control_loops/superstructure/superstructure.h"
+
+#include <unistd.h>
+
+#include <chrono>
+#include <memory>
+
+#include "aos/common/controls/control_loop_test.h"
+#include "aos/common/queue.h"
+#include "frc971/control_loops/position_sensor_sim.h"
+#include "frc971/control_loops/team_number_test_environment.h"
+#include "gtest/gtest.h"
+#include "y2018/constants.h"
+#include "y2018/control_loops/superstructure/intake/intake_plant.h"
+
+using ::frc971::control_loops::PositionSensorSimulator;
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace testing {
+namespace {
+constexpr double kNoiseScalar = 0.01;
+} // namespace
+
+namespace chrono = ::std::chrono;
+using ::aos::monotonic_clock;
+
+class IntakePlant : public StateFeedbackPlant<5, 1, 2> {
+ public:
+ explicit IntakePlant(StateFeedbackPlant<5, 1, 2> &&other)
+ : StateFeedbackPlant<5, 1, 2>(::std::move(other)) {}
+
+ void CheckU(const ::Eigen::Matrix<double, 1, 1> &U) override {
+ EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + voltage_offset_);
+ EXPECT_GE(U(0, 0), U_min(0, 0) - 0.00001 + voltage_offset_);
+ }
+
+ double voltage_offset() const { return voltage_offset_; }
+ void set_voltage_offset(double voltage_offset) {
+ voltage_offset_ = voltage_offset;
+ }
+
+ private:
+ double voltage_offset_ = 0.0;
+};
+
+class IntakeSideSimulation {
+ public:
+ explicit IntakeSideSimulation(
+ StateFeedbackPlant<5, 1, 2> &&other,
+ const ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
+ &zeroing_constants)
+ : plant_(::std::move(other)),
+ pot_encoder_(M_PI * 2.0 *
+ constants::Values::kIntakeMotorEncoderRatio()),
+ zeroing_constants_(zeroing_constants) {}
+
+ void InitializePosition(double start_pos) {
+ plant_.mutable_X().setZero();
+ plant_.mutable_X(0) = start_pos;
+ plant_.mutable_X(2) = start_pos;
+
+ pot_encoder_.Initialize(
+ start_pos, kNoiseScalar, 0.0,
+ zeroing_constants_.measured_absolute_position);
+ }
+
+ void GetSensorValues(IntakeElasticSensors *position) {
+ pot_encoder_.GetSensorValues(&position->motor_position);
+ position->spring_angle = plant_.Y(0);
+ }
+
+ double spring_position() const { return plant_.X(0); }
+ double spring_velocity() const { return plant_.X(1); }
+ double motor_position() const { return plant_.X(2); }
+ double motor_velocity() const { return plant_.X(3); }
+
+ void set_voltage_offset(double voltage_offset) {
+ plant_.set_voltage_offset(voltage_offset);
+ }
+
+ void Simulate(const IntakeVoltage &intake_voltage) {
+ const double voltage_check =
+ superstructure::intake::IntakeSide::kOperatingVoltage();
+
+ CHECK_LE(::std::abs(intake_voltage.voltage_elastic), voltage_check);
+
+ ::Eigen::Matrix<double, 1, 1> U;
+ U << intake_voltage.voltage_elastic + plant_.voltage_offset();
+
+ plant_.Update(U);
+
+ const double position_intake = plant_.Y(1);
+
+ pot_encoder_.MoveTo(position_intake);
+ EXPECT_GE(position_intake,
+ constants::Values::kIntakeRange().lower_hard);
+ EXPECT_LE(position_intake,
+ constants::Values::kIntakeRange().upper_hard);
+ }
+
+ private:
+ IntakePlant plant_;
+
+ PositionSensorSimulator pot_encoder_;
+
+ const ::frc971::constants::PotAndAbsoluteEncoderZeroingConstants
+ zeroing_constants_;
+};
+
+class SuperstructureSimulation {
+ public:
+ SuperstructureSimulation()
+ : left_intake_(::y2018::control_loops::superstructure::intake::
+ MakeDelayedIntakePlant(),
+ constants::GetValues().left_intake.zeroing),
+ right_intake_(::y2018::control_loops::superstructure::intake::
+ MakeDelayedIntakePlant(),
+ constants::GetValues().right_intake.zeroing),
+ superstructure_queue_(".y2018.control_loops.superstructure", 0xdeadbeef,
+ ".y2018.control_loops.superstructure.goal",
+ ".y2018.control_loops.superstructure.position",
+ ".y2018.control_loops.superstructure.output",
+ ".y2018.control_loops.superstructure.status") {
+ // Start the intake out in the middle by default.
+ InitializeIntakePosition((constants::Values::kIntakeRange().lower +
+ constants::Values::kIntakeRange().upper) /
+ 2.0);
+ }
+
+ void InitializeIntakePosition(double start_pos) {
+ left_intake_.InitializePosition(start_pos);
+ right_intake_.InitializePosition(start_pos);
+ }
+
+ void SendPositionMessage() {
+ ::aos::ScopedMessagePtr<SuperstructureQueue::Position> position =
+ superstructure_queue_.position.MakeMessage();
+
+ left_intake_.GetSensorValues(&position->intake.left);
+ right_intake_.GetSensorValues(&position->intake.right);
+ LOG_STRUCT(INFO, "sim position", *position);
+ position.Send();
+ }
+
+ // Sets the difference between the commanded and applied powers.
+ // This lets us test that the integrators work.
+ void set_left_intake_voltage_offset(double voltage_offset) {
+ left_intake_.set_voltage_offset(voltage_offset);
+ }
+ void set_right_intake_voltage_offset(double voltage_offset) {
+ right_intake_.set_voltage_offset(voltage_offset);
+ }
+
+ const IntakeSideSimulation &left_intake() const { return left_intake_; }
+ const IntakeSideSimulation &right_intake() const { return right_intake_; }
+
+ // Simulates the intake for a single timestep.
+ void Simulate() {
+ EXPECT_TRUE(superstructure_queue_.output.FetchLatest());
+ EXPECT_TRUE(superstructure_queue_.status.FetchLatest());
+
+ left_intake_.Simulate(superstructure_queue_.output->intake.left);
+ right_intake_.Simulate(superstructure_queue_.output->intake.right);
+ }
+
+ private:
+ IntakeSideSimulation left_intake_;
+ IntakeSideSimulation right_intake_;
+
+ SuperstructureQueue superstructure_queue_;
+};
+
+class SuperstructureTest : public ::aos::testing::ControlLoopTest {
+ protected:
+ SuperstructureTest()
+ : superstructure_queue_(".y2018.control_loops.superstructure", 0xdeadbeef,
+ ".y2018.control_loops.superstructure.goal",
+ ".y2018.control_loops.superstructure.position",
+ ".y2018.control_loops.superstructure.output",
+ ".y2018.control_loops.superstructure.status"),
+ superstructure_(&superstructure_queue_) {
+ set_team_id(::frc971::control_loops::testing::kTeamNumber);
+ }
+
+ void VerifyNearGoal() {
+ superstructure_queue_.goal.FetchLatest();
+ superstructure_queue_.status.FetchLatest();
+
+ ASSERT_TRUE(superstructure_queue_.goal.get() != nullptr);
+ ASSERT_TRUE(superstructure_queue_.status.get() != nullptr);
+ // Left side test.
+ EXPECT_NEAR(superstructure_queue_.goal->intake.left_intake_angle,
+ superstructure_queue_.status->left_intake.spring_position,
+ 0.001);
+ EXPECT_NEAR(superstructure_queue_.goal->intake.left_intake_angle,
+ superstructure_plant_.left_intake().spring_position(), 0.001);
+
+ // Right side test.
+ EXPECT_NEAR(superstructure_queue_.goal->intake.right_intake_angle,
+ superstructure_queue_.status->right_intake.spring_position,
+ 0.001);
+ EXPECT_NEAR(superstructure_queue_.goal->intake.right_intake_angle,
+ superstructure_plant_.right_intake().spring_position(), 0.001);
+ }
+
+ // Runs one iteration of the whole simulation.
+ void RunIteration(bool enabled = true) {
+ SendMessages(enabled);
+
+ superstructure_plant_.SendPositionMessage();
+ superstructure_.Iterate();
+ superstructure_plant_.Simulate();
+
+ TickTime(::std::chrono::microseconds(5050));
+ }
+
+ // Runs iterations until the specified amount of simulated time has elapsed.
+ void RunForTime(const monotonic_clock::duration run_for,
+ bool enabled = true) {
+ const auto end_time = monotonic_clock::now() + run_for;
+ while (monotonic_clock::now() < end_time) {
+ RunIteration(enabled);
+ }
+ }
+
+ // Create a new instance of the test queue so that it invalidates the queue
+ // that it points to. Otherwise, we will have a pointer to shared memory
+ // that is no longer valid.
+ SuperstructureQueue superstructure_queue_;
+
+ // Create a control loop and simulation.
+ Superstructure superstructure_;
+ SuperstructureSimulation superstructure_plant_;
+};
+
+// Tests that the loop zeroes when run for a while without a goal.
+TEST_F(SuperstructureTest, ZeroNoGoalAndDoesNothing) {
+ RunForTime(chrono::seconds(2));
+ superstructure_queue_.output.FetchLatest();
+
+ EXPECT_EQ(intake::IntakeSide::State::RUNNING,
+ superstructure_.intake_left().state());
+ EXPECT_EQ(intake::IntakeSide::State::RUNNING,
+ superstructure_.intake_right().state());
+ EXPECT_EQ(superstructure_queue_.output->intake.left.voltage_elastic, 0.0);
+ EXPECT_EQ(superstructure_queue_.output->intake.right.voltage_elastic, 0.0);
+}
+
+// Tests that the intake loop can reach a goal.
+TEST_F(SuperstructureTest, ReachesGoal) {
+ // Set a reasonable goal.
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+
+ goal->intake.left_intake_angle = 0.1;
+ goal->intake.right_intake_angle = 0.2;
+
+ ASSERT_TRUE(goal.Send());
+ }
+
+ // Give it a lot of time to get there.
+ RunForTime(chrono::seconds(8));
+
+ VerifyNearGoal();
+}
+
+// Tests that the intake loop can reach a goal after starting at a non-zero
+// position.
+TEST_F(SuperstructureTest, OffsetStartReachesGoal) {
+ superstructure_plant_.InitializeIntakePosition(0.5);
+
+ // Set a reasonable goal.
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+
+ goal->intake.left_intake_angle = 0.1;
+ goal->intake.right_intake_angle = 0.2;
+
+ ASSERT_TRUE(goal.Send());
+ }
+
+ // Give it a lot of time to get there.
+ RunForTime(chrono::seconds(8));
+
+ VerifyNearGoal();
+}
+
+// Tests that the intake loops doesn't try and go beyond the
+// physical range of the mechanisms.
+TEST_F(SuperstructureTest, RespectsRange) {
+ // Set some ridiculous goals to test upper limits.
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+
+ goal->intake.left_intake_angle = 5.0 * M_PI;
+ goal->intake.right_intake_angle = 5.0 * M_PI;
+
+ ASSERT_TRUE(goal.Send());
+ }
+ RunForTime(chrono::seconds(10));
+
+ // Check that we are near our soft limit.
+ superstructure_queue_.status.FetchLatest();
+
+ EXPECT_NEAR(constants::Values::kIntakeRange().upper,
+ superstructure_queue_.status->left_intake.spring_position,
+ 0.001);
+
+ EXPECT_NEAR(constants::Values::kIntakeRange().upper,
+ superstructure_queue_.status->right_intake.spring_position,
+ 0.001);
+
+ // Set some ridiculous goals to test lower limits.
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+
+ goal->intake.left_intake_angle = -5.0 * M_PI;
+ goal->intake.right_intake_angle = -5.0 * M_PI;
+
+ ASSERT_TRUE(goal.Send());
+ }
+
+ RunForTime(chrono::seconds(10));
+
+ // Check that we are near our soft limit.
+ superstructure_queue_.status.FetchLatest();
+
+ EXPECT_NEAR(constants::Values::kIntakeRange().lower,
+ superstructure_queue_.status->left_intake.spring_position,
+ 0.001);
+
+ EXPECT_NEAR(constants::Values::kIntakeRange().lower,
+ superstructure_queue_.status->right_intake.spring_position,
+ 0.001);}
+
+TEST_F(SuperstructureTest, DISABLED_LowerHardstopStartup) {
+ superstructure_plant_.InitializeIntakePosition(
+ constants::Values::kIntakeRange().lower_hard);
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->intake.left_intake_angle = constants::Values::kIntakeRange().lower;
+ goal->intake.right_intake_angle = constants::Values::kIntakeRange().lower;
+ ASSERT_TRUE(goal.Send());
+ }
+ RunForTime(chrono::seconds(10));
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->intake.left_intake_angle = 1.0;
+ goal->intake.right_intake_angle = 1.0;
+ ASSERT_TRUE(goal.Send());
+ }
+ RunForTime(chrono::seconds(10));
+ VerifyNearGoal();
+}
+
+// Tests that starting at the upper hardstops doesn't cause an abort.
+TEST_F(SuperstructureTest, DISABLED_UpperHardstopStartup) {
+ superstructure_plant_.InitializeIntakePosition(
+ constants::Values::kIntakeRange().upper_hard);
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->intake.left_intake_angle = constants::Values::kIntakeRange().upper;
+ goal->intake.right_intake_angle = constants::Values::kIntakeRange().upper;
+ ASSERT_TRUE(goal.Send());
+ }
+ RunForTime(chrono::seconds(10));
+
+ VerifyNearGoal();
+}
+
+// Tests that resetting WPILib results in a rezero.
+TEST_F(SuperstructureTest, ResetTest) {
+ superstructure_plant_.InitializeIntakePosition(
+ constants::Values::kIntakeRange().upper);
+ {
+ auto goal = superstructure_queue_.goal.MakeMessage();
+ goal->intake.left_intake_angle =
+ constants::Values::kIntakeRange().upper - 0.1;
+ goal->intake.right_intake_angle =
+ constants::Values::kIntakeRange().upper - 0.1;
+ ASSERT_TRUE(goal.Send());
+ }
+ RunForTime(chrono::seconds(10));
+
+ EXPECT_EQ(intake::IntakeSide::State::RUNNING,
+ superstructure_.intake_left().state());
+ EXPECT_EQ(intake::IntakeSide::State::RUNNING,
+ superstructure_.intake_right().state());
+
+ VerifyNearGoal();
+ SimulateSensorReset();
+ RunForTime(chrono::milliseconds(100));
+
+ EXPECT_EQ(intake::IntakeSide::State::ZEROING,
+ superstructure_.intake_left().state());
+ EXPECT_EQ(intake::IntakeSide::State::ZEROING,
+ superstructure_.intake_right().state());
+
+ RunForTime(chrono::seconds(10));
+
+ EXPECT_EQ(intake::IntakeSide::State::RUNNING,
+ superstructure_.intake_left().state());
+ EXPECT_EQ(intake::IntakeSide::State::RUNNING,
+ superstructure_.intake_right().state());
+
+ VerifyNearGoal();
+}
+
+} // namespace testing
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2018
diff --git a/y2018/control_loops/superstructure/superstructure_main.cc b/y2018/control_loops/superstructure/superstructure_main.cc
new file mode 100644
index 0000000..04d245e
--- /dev/null
+++ b/y2018/control_loops/superstructure/superstructure_main.cc
@@ -0,0 +1,11 @@
+#include "y2018/control_loops/superstructure/superstructure.h"
+
+#include "aos/linux_code/init.h"
+
+int main() {
+ ::aos::Init();
+ ::y2018::control_loops::superstructure::Superstructure superstructure;
+ superstructure.Run();
+ ::aos::Cleanup();
+ return 0;
+}