Move shared flywheel code into frc971
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Ieac317a3e5bc8243e63473f485a2467b74aed348
diff --git a/frc971/control_loops/flywheel/flywheel_controller.h b/frc971/control_loops/flywheel/flywheel_controller.h
new file mode 100644
index 0000000..db8baeb
--- /dev/null
+++ b/frc971/control_loops/flywheel/flywheel_controller.h
@@ -0,0 +1,80 @@
+#ifndef FRC971_CONTROL_LOOPS_SHOOTER_FLYWHEEL_CONTROLLER_H_
+#define FRC971_CONTROL_LOOPS_SHOOTER_FLYWHEEL_CONTROLLER_H_
+
+#include <memory>
+
+#include "aos/time/time.h"
+#include "frc971/control_loops/control_loop.h"
+#include "frc971/control_loops/flywheel/flywheel_controller_status_generated.h"
+#include "frc971/control_loops/hybrid_state_feedback_loop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace flywheel {
+
+class CurrentLimitedStateFeedbackController;
+
+// Handles the velocity control of each flywheel.
+class FlywheelController {
+ public:
+ FlywheelController(
+ StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>> &&loop,
+ double bemf, double resistance);
+
+ ~FlywheelController();
+
+ // Sets the velocity goal in radians/sec
+ void set_goal(double angular_velocity_goal);
+ double goal() const { return last_goal_; }
+ // Sets the current encoder position in radians
+ void set_position(double current_position,
+ const aos::monotonic_clock::time_point position_timestamp);
+
+ // Populates the status structure.
+ flatbuffers::Offset<FlywheelControllerStatus> SetStatus(
+ flatbuffers::FlatBufferBuilder *fbb);
+
+ // Returns the control loop calculated voltage.
+ double voltage() const;
+
+ // Returns the expected battery current for the last U.
+ double current() const;
+
+ // Returns the instantaneous velocity.
+ double velocity() const;
+
+ // Executes the control loop for a cycle.
+ void Update(bool disabled);
+
+ double avg_angular_velocity() { return avg_angular_velocity_; }
+
+ private:
+ // The current sensor measurement.
+ Eigen::Matrix<double, 1, 1> Y_;
+ // The control loop.
+ ::std::unique_ptr<CurrentLimitedStateFeedbackController> loop_;
+
+ // History array for calculating a filtered angular velocity.
+ static constexpr int kHistoryLength = 10;
+ ::std::array<std::pair<double, ::aos::monotonic_clock::time_point>,
+ kHistoryLength>
+ history_;
+ ptrdiff_t history_position_ = 0;
+
+ // Average velocity logging.
+ double avg_angular_velocity_ = 0;
+
+ double last_goal_ = 0;
+
+ bool first_ = true;
+
+ DISALLOW_COPY_AND_ASSIGN(FlywheelController);
+};
+
+} // namespace flywheel
+} // namespace control_loops
+} // namespace frc971
+
+#endif // FRC971_CONTROL_LOOPS_SHOOTER_FLYWHEEL_CONTROLLER_H_