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