Began shooter integration in superstructure class.
This adds the shooter control loops to the superstructure class.
Change-Id: Ia54550a8c9598349347df7df8e07d81becd1673e
diff --git a/y2017/control_loops/superstructure/shooter/shooter.h b/y2017/control_loops/superstructure/shooter/shooter.h
new file mode 100644
index 0000000..7f11267
--- /dev/null
+++ b/y2017/control_loops/superstructure/shooter/shooter.h
@@ -0,0 +1,101 @@
+#ifndef Y2017_CONTROL_LOOPS_SHOOTER_SHOOTER_H_
+#define Y2017_CONTROL_LOOPS_SHOOTER_SHOOTER_H_
+
+#include <memory>
+
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/time.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+
+#include "y2017/control_loops/superstructure/shooter/shooter_integral_plant.h"
+#include "y2017/control_loops/superstructure/superstructure.q.h"
+
+namespace y2017 {
+namespace control_loops {
+namespace superstructure {
+namespace shooter {
+
+class ShooterController {
+ public:
+ ShooterController();
+
+ // Sets the velocity goal in radians/sec
+ void set_goal(double angular_velocity_goal);
+ // Sets the current encoder position in radians
+ void set_position(double current_position);
+
+ // Populates the status structure.
+ void SetStatus(control_loops::ShooterStatus *status);
+
+ // Returns the control loop calculated voltage.
+ double voltage() const;
+
+ // Returns the instantaneous velocity.
+ double velocity() const { return loop_->X_hat(1, 0); }
+
+ double dt_velocity() const { return dt_velocity_; }
+
+ double error() const { return error_; }
+
+ // Executes the control loop for a cycle.
+ void Update(bool disabled);
+
+ // Resets the kalman filter and any other internal state.
+ void Reset();
+
+ private:
+ // The current sensor measurement.
+ Eigen::Matrix<double, 1, 1> Y_;
+ // The control loop.
+ ::std::unique_ptr<StateFeedbackLoop<3, 1, 1>> loop_;
+
+ // History array for calculating a filtered angular velocity.
+ static constexpr int kHistoryLength = 5;
+ ::std::array<double, kHistoryLength> history_;
+ ptrdiff_t history_position_ = 0;
+
+ double error_ = 0.0;
+ double dt_velocity_ = 0.0;
+ double last_position_ = 0.0;
+ double average_angular_velocity_ = 0.0;
+ double min_velocity_ = 0.0;
+ double position_error_ = 0.0;
+
+ Eigen::Matrix<double, 3, 1> X_hat_current_;
+
+ bool ready_ = false;
+ bool needs_reset_ = false;
+ bool reset_ = false;
+
+ bool last_ready_ = false;
+ DISALLOW_COPY_AND_ASSIGN(ShooterController);
+};
+
+class Shooter {
+ public:
+ Shooter() {}
+
+ // Iterates the shooter control loop one cycle. position and status must
+ // never be nullptr. goal can be nullptr if no goal exists, and output should
+ // be nullptr if disabled.
+ void Iterate(const control_loops::ShooterGoal *goal, const double *position,
+ double *output, control_loops::ShooterStatus *status);
+
+ // Sets the shooter up to reset the kalman filter next time Iterate is called.
+ void Reset();
+
+ private:
+ ShooterController wheel_;
+
+ bool last_ready_ = false;
+ double min_ = 0.0;
+
+ DISALLOW_COPY_AND_ASSIGN(Shooter);
+};
+
+} // namespace shooter
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2017
+
+#endif // Y2017_CONTROL_LOOPS_SHOOTER_SHOOTER_H_