y2017/control_loops/superstructure/column/column.h

#ifndef Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_COLUMN_COLUMN_H_
#define Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_COLUMN_COLUMN_H_

#include <array>
#include <chrono>
#include <memory>
#include <utility>

#include "Eigen/Dense"

#include "frc971/constants.h"
#include "frc971/control_loops/profiled_subsystem.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "y2017/constants.h"
#include "y2017/control_loops/superstructure/column/column_zeroing.h"
#include "y2017/control_loops/superstructure/intake/intake.h"
#include "y2017/control_loops/superstructure/superstructure_position_generated.h"
#include "y2017/control_loops/superstructure/superstructure_status_generated.h"
#include "y2017/control_loops/superstructure/vision_time_adjuster.h"
#include "y2017/vision/vision_generated.h"

namespace y2017::control_loops::superstructure::column {

class ColumnProfiledSubsystem
    : public ::frc971::control_loops::ProfiledSubsystem<
          6, 1, ColumnZeroingEstimator, 2, 2> {
 public:
  ColumnProfiledSubsystem(
      ::std::unique_ptr<
          ::frc971::control_loops::SimpleCappedStateFeedbackLoop<6, 2, 2>>
          loop,
      const ::y2017::constants::Values::Column &zeroing_constants,
      const ::frc971::constants::Range &range, double default_angular_velocity,
      double default_angular_acceleration);

  // Updates our estimator with the latest position.
  void Correct(const ColumnPosition &position);
  // Runs the controller and profile generator for a cycle.
  void Update(bool disabled);

  // Fills out the ProfiledJointStatus structure with the current state.
  void PopulateTurretStatus(
      TurretProfiledSubsystemStatus::Builder *status_builder,
      flatbuffers::Offset<ColumnZeroingEstimator::State>
          estimator_state_offset);

  // Forces the current goal to the provided goal, bypassing the profiler.
  void ForceGoal(double goal_velocity, double goal);
  // Sets the unprofiled goal.  The profiler will generate a profile to go to
  // this goal.
  void set_indexer_unprofiled_goal(double goal_velocity);
  void set_turret_unprofiled_goal(double unprofiled_goal);
  void set_unprofiled_goal(double goal_velocity, double unprofiled_goal);
  // Limits our profiles to a max velocity and acceleration for proper motion.
  void AdjustProfile(const ::frc971::ProfileParameters *profile_parameters);
  void AdjustProfile(double max_angular_velocity,
                     double max_angular_acceleration);

  // Returns true if we have exceeded any hard limits.
  bool CheckHardLimits();

  // Returns the requested voltage.
  double indexer_voltage() const { return loop_->U(0, 0); }
  double uncapped_indexer_voltage() const { return loop_->U_uncapped(0, 0); }
  double turret_voltage() const { return loop_->U(1, 0); }

  // Returns the current Y.
  const ::Eigen::Matrix<double, 2, 1> Y() const { return Y_; }
  double Y(int i, int j) const { return Y_(i, j); }

  // Returns the current indexer position.
  double indexer_position() const { return Y_(0, 0); }

  bool saturated() const { return saturated_; }

  // Returns the current turret position.
  double turret_position() const { return Y_(1, 0) + Y_(0, 0); }

  // For testing:
  // Triggers an estimator error.
  void TriggerEstimatorError() { estimators_[0].TriggerError(); }

  const ::frc971::constants::Range &range() const { return range_; }

  bool IsIndexerStuck() const;
  double IndexerStuckVoltage() const;
  void PartialIndexerReset();
  void PartialTurretReset();
  void PopulateIndexerStatus(IndexerStatus::Builder *status_builder);

  void AddOffset(double indexer_offset_delta, double turret_offset_delta);

 protected:
  // Limits the provided goal to the soft limits.  Prints "name" when it fails
  // to aid debugging.
  virtual void CapGoal(const char *name, Eigen::Matrix<double, 6, 1> *goal);

 private:
  void UpdateOffset(double indexer_offset, double turret_offset);

  ::std::unique_ptr<StateFeedbackLoop<6, 2, 2>> stuck_indexer_detector_;

  // History array for calculating a filtered angular velocity.
  static constexpr int kHistoryLength = 5;
  ::std::array<double, kHistoryLength> indexer_history_;
  ptrdiff_t indexer_history_position_ = 0;

  double indexer_error_ = 0.0;
  double indexer_dt_velocity_ = 0.0;
  double indexer_last_position_ = 0.0;
  double indexer_average_angular_velocity_ = 0.0;
  double indexer_position_error_ = 0.0;
  bool indexer_ready_ = false;

  bool saturated_ = false;

  Eigen::Matrix<double, 6, 1> X_hat_current_;
  Eigen::Matrix<double, 6, 1> stuck_indexer_X_hat_current_;

  aos::util::TrapezoidProfile profile_;

  // Current measurement.
  Eigen::Matrix<double, 2, 1> Y_;
  // Current offset.  Y_ = offset_ + raw_sensor;
  Eigen::Matrix<double, 2, 1> offset_;

  const ::frc971::constants::Range range_;

  const double default_velocity_;
  const double default_acceleration_;

  double turret_last_position_ = 0;
};

class Column {
 public:
  Column(::aos::EventLoop *event_loop);
  double goal(int row, int col) const {
    return profiled_subsystem_.goal(row, col);
  }

  double turret_position() const {
    return profiled_subsystem_.turret_position();
  }

  void set_freeze(bool freeze) { freeze_ = freeze; }

  // The zeroing and operating voltages.
  static constexpr double kZeroingVoltage = 5.0;
  static constexpr double kOperatingVoltage = 12.0;
  static constexpr double kIntakeZeroingMinDistance = 0.08;
  static constexpr double kIntakeTolerance = 0.005;
  static constexpr double kStuckZeroingTrackingError = 0.02;
  static constexpr double kTurretMin = -0.1;
  static constexpr double kTurretMax = M_PI / 2.0 + 0.1;

  std::pair<flatbuffers::Offset<IndexerStatus>,
            flatbuffers::Offset<TurretProfiledSubsystemStatus>>
  Iterate(::aos::monotonic_clock::time_point monotonic_now,
          const IndexerGoalT *unsafe_indexer_goal,
          const TurretGoal *unsafe_turret_goal, const ColumnPosition *position,
          const vision::VisionStatus *vision_status, double *indexer_output,
          double *turret_output, flatbuffers::FlatBufferBuilder *fbb,
          // IndexerStatus *indexer_status,
          // TurretProfiledSubsystemStatus *turret_status,
          intake::Intake *intake);

  void Reset();

  enum class State : int32_t {
    UNINITIALIZED = 0,
    ZEROING_UNINITIALIZED = 1,
    ZEROING_POSITIVE = 2,
    ZEROING_NEGATIVE = 3,
    RUNNING = 4,
    ESTOP = 5,
  };

  enum class IndexerState : int32_t {
    // The system is running correctly, no stuck condition detected.
    RUNNING = 0,
    // The system is reversing to unjam.
    REVERSING = 1
  };

  State state() const { return state_; }
  IndexerState indexer_state() const { return indexer_state_; }

 private:
  State state_ = State::UNINITIALIZED;

  IndexerState indexer_state_ = IndexerState::RUNNING;

  bool freeze_ = false;

  VisionTimeAdjuster vision_time_adjuster_;

  // The last time that we transitioned from RUNNING to REVERSING or the
  // reverse.  Used to implement the timeouts.
  ::aos::monotonic_clock::time_point last_transition_time_ =
      ::aos::monotonic_clock::min_time;

  ColumnProfiledSubsystem profiled_subsystem_;

  const double vision_error_;
};

}  // namespace y2017::control_loops::superstructure::column

#endif  // Y2017_CONTROL_LOOPS_SUPERSTRUCTURE_COLUMN_COLUMN_H_