Move 2015-specific code to its own folder.
Known issues:
-I didn't change the namespace for it, but I am open to discussion
on doing that in a separate change.
-There are a couple of files which should get split out into
year-specific and not-year-specific files to reduce how much needs
to get copied around each year still.
-The control loop python code doesn't yet generate code with the
right #include etc paths.
Change-Id: Iabf078e75107c283247f58a5ffceb4dbd6a0815f
diff --git a/y2015/control_loops/fridge/fridge.h b/y2015/control_loops/fridge/fridge.h
new file mode 100644
index 0000000..0448e09
--- /dev/null
+++ b/y2015/control_loops/fridge/fridge.h
@@ -0,0 +1,172 @@
+#ifndef Y2015_CONTROL_LOOPS_FRIDGE_H_
+#define Y2015_CONTROL_LOOPS_FRIDGE_H_
+
+#include <memory>
+
+#include "aos/common/controls/control_loop.h"
+#include "aos/common/util/trapezoid_profile.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+#include "y2015/control_loops/fridge/fridge.q.h"
+#include "frc971/zeroing/zeroing.h"
+#include "y2015/util/kinematics.h"
+
+namespace frc971 {
+namespace control_loops {
+namespace testing {
+class FridgeTest_DisabledGoalTest_Test;
+class FridgeTest_ArmGoalPositiveWindupTest_Test;
+class FridgeTest_ElevatorGoalPositiveWindupTest_Test;
+class FridgeTest_ArmGoalNegativeWindupTest_Test;
+class FridgeTest_ElevatorGoalNegativeWindupTest_Test;
+class FridgeTest_SafeArmZeroing_Test;
+} // namespace testing
+
+template<int S>
+class CappedStateFeedbackLoop : public StateFeedbackLoop<S, 2, 2> {
+ public:
+ CappedStateFeedbackLoop(StateFeedbackLoop<S, 2, 2> &&loop)
+ : StateFeedbackLoop<S, 2, 2>(::std::move(loop)), max_voltage_(12.0) {}
+
+ void set_max_voltage(double max_voltage) {
+ max_voltage_ = ::std::max(0.0, ::std::min(12.0, max_voltage));
+ }
+
+ void CapU() override;
+
+ // Returns the amount to change the position goals (average and difference) in
+ // order to no longer saturate the controller.
+ Eigen::Matrix<double, 2, 1> UnsaturateOutputGoalChange();
+
+ private:
+ double max_voltage_;
+};
+
+class Fridge
+ : public aos::controls::ControlLoop<control_loops::FridgeQueue> {
+ public:
+ explicit Fridge(
+ control_loops::FridgeQueue *fridge_queue = &control_loops::fridge_queue);
+
+ enum State {
+ // Waiting to receive data before doing anything.
+ UNINITIALIZED = 0,
+ // Estimating the starting location.
+ INITIALIZING = 1,
+ // Moving the elevator to find an index pulse.
+ ZEROING_ELEVATOR = 2,
+ // Moving the arm to find an index pulse.
+ ZEROING_ARM = 3,
+ // All good!
+ RUNNING = 4,
+ // Internal error caused the fridge to abort.
+ ESTOP = 5,
+ };
+
+ enum class ProfilingType : int32_t {
+ // Use angle/height to specify the fridge goal.
+ ANGLE_HEIGHT_PROFILING = 0,
+ // Use x/y co-ordinates to specify the fridge goal.
+ X_Y_PROFILING = 1,
+ };
+
+ State state() const { return state_; }
+
+ protected:
+ void RunIteration(const control_loops::FridgeQueue::Goal *goal,
+ const control_loops::FridgeQueue::Position *position,
+ control_loops::FridgeQueue::Output *output,
+ control_loops::FridgeQueue::Status *status) override;
+
+ private:
+ friend class testing::FridgeTest_DisabledGoalTest_Test;
+ friend class testing::FridgeTest_ElevatorGoalPositiveWindupTest_Test;
+ friend class testing::FridgeTest_ArmGoalPositiveWindupTest_Test;
+ friend class testing::FridgeTest_ElevatorGoalNegativeWindupTest_Test;
+ friend class testing::FridgeTest_ArmGoalNegativeWindupTest_Test;
+ friend class testing::FridgeTest_SafeArmZeroing_Test;
+
+ // Sets state_ to the correct state given the current state of the zeroing
+ // estimators.
+ void UpdateZeroingState();
+
+ void SetElevatorOffset(double left_offset, double right_offset);
+ void SetArmOffset(double left_offset, double right_offset);
+
+ // Getters for the current elevator positions.
+ double left_elevator();
+ double right_elevator();
+ double elevator();
+
+ // Getters for the current arm positions.
+ double left_arm();
+ double right_arm();
+ double arm();
+
+ // Our best guess at the current position of the elevator.
+ double estimated_left_elevator();
+ double estimated_right_elevator();
+ double estimated_elevator();
+
+ // Our best guess at the current position of the arm.
+ double estimated_left_arm();
+ double estimated_right_arm();
+ double estimated_arm();
+
+ // Returns the current zeroing velocity for either subsystem.
+ // If the subsystem is too far away from the center, these will switch
+ // directions.
+ double elevator_zeroing_velocity();
+ double arm_zeroing_velocity();
+
+ // Corrects the Observer with the current position.
+ void Correct();
+
+ double UseUnlessZero(double target_value, double default_value);
+
+ // The state feedback control loop or loops to talk to.
+ ::std::unique_ptr<CappedStateFeedbackLoop<5>> arm_loop_;
+ ::std::unique_ptr<CappedStateFeedbackLoop<4>> elevator_loop_;
+
+ zeroing::ZeroingEstimator left_arm_estimator_;
+ zeroing::ZeroingEstimator right_arm_estimator_;
+ zeroing::ZeroingEstimator left_elevator_estimator_;
+ zeroing::ZeroingEstimator right_elevator_estimator_;
+
+ // Offsets from the encoder position to the absolute position. Add these to
+ // the encoder position to get the absolute position.
+ double left_elevator_offset_ = 0.0;
+ double right_elevator_offset_ = 0.0;
+ double left_arm_offset_ = 0.0;
+ double right_arm_offset_ = 0.0;
+
+ // Current velocity to move at while zeroing.
+ double elevator_zeroing_velocity_ = 0.0;
+ double arm_zeroing_velocity_ = 0.0;
+
+ // The goals for the elevator and arm.
+ double elevator_goal_ = 0.0;
+ double arm_goal_ = 0.0;
+
+ double arm_goal_velocity_ = 0.0;
+ double elevator_goal_velocity_ = 0.0;
+
+ State state_ = UNINITIALIZED;
+ State last_state_ = UNINITIALIZED;
+
+ control_loops::FridgeQueue::Position current_position_;
+
+ ProfilingType last_profiling_type_;
+ aos::util::ElevatorArmKinematics kinematics_;
+
+ aos::util::TrapezoidProfile arm_profile_;
+ aos::util::TrapezoidProfile elevator_profile_;
+
+ aos::util::TrapezoidProfile x_profile_;
+ aos::util::TrapezoidProfile y_profile_;
+};
+
+} // namespace control_loops
+} // namespace frc971
+
+#endif // Y2015_CONTROL_LOOPS_FRIDGE_H_
+