Added a start at a fridge.
Change-Id: I5bab9686f966368161ede528ff7abf038d7bcb9a
diff --git a/frc971/control_loops/fridge/fridge.h b/frc971/control_loops/fridge/fridge.h
index 3d0fa6c..5ee6ed0 100644
--- a/frc971/control_loops/fridge/fridge.h
+++ b/frc971/control_loops/fridge/fridge.h
@@ -8,10 +8,30 @@
#include "frc971/control_loops/fridge/fridge.q.h"
#include "frc971/control_loops/fridge/arm_motor_plant.h"
#include "frc971/control_loops/fridge/elevator_motor_plant.h"
+#include "frc971/zeroing/zeroing.h"
namespace frc971 {
namespace control_loops {
+class CappedStateFeedbackLoop : public StateFeedbackLoop<4, 2, 2> {
+ public:
+ CappedStateFeedbackLoop(StateFeedbackLoop<4, 2, 2> &&loop)
+ : StateFeedbackLoop<4, 2, 2>(::std::move(loop)), max_voltage_(12.0) {}
+
+ void set_max_voltage(double max_voltage) {
+ max_voltage_ = ::std::max(-12.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:
@@ -28,16 +48,92 @@
//static constexpr double dt;
protected:
- virtual void RunIteration(
- const control_loops::FridgeQueue::Goal *goal,
- const control_loops::FridgeQueue::Position *position,
- control_loops::FridgeQueue::Output *output,
- control_loops::FridgeQueue::Status *status);
+ 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:
+ // 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();
+
+ 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,
+ };
+
// The state feedback control loop or loops to talk to.
- ::std::unique_ptr<StateFeedbackLoop<4, 2, 2>> arm_loop_;
- ::std::unique_ptr<StateFeedbackLoop<4, 2, 2>> elev_loop_;
+ ::std::unique_ptr<CappedStateFeedbackLoop> arm_loop_;
+ ::std::unique_ptr<CappedStateFeedbackLoop> 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;
+
+ State state_ = UNINITIALIZED;
+ State last_state_ = UNINITIALIZED;
+
+ control_loops::FridgeQueue::Position current_position_;
+ static constexpr double dt = 0.005;
};
} // namespace control_loops