Add claw control loop.
It's based pretty heavily on the fridge loop. Currently, all the
tests pass.
Change-Id: Ieb386dfa5c3fe2d34e2d191fa39a44dc77ee6ab6
diff --git a/frc971/control_loops/claw/claw.h b/frc971/control_loops/claw/claw.h
index 3d7e647..6abc082 100644
--- a/frc971/control_loops/claw/claw.h
+++ b/frc971/control_loops/claw/claw.h
@@ -4,28 +4,60 @@
#include <memory>
#include "aos/common/controls/control_loop.h"
+#include "aos/common/time.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/control_loops/claw/claw.q.h"
#include "frc971/control_loops/claw/claw_motor_plant.h"
+#include "frc971/zeroing/zeroing.h"
namespace frc971 {
namespace control_loops {
+namespace testing {
+class ClawTest_DisabledGoal_Test;
+class ClawTest_GoalPositiveWindup_Test;
+class ClawTest_GoalNegativeWindup_Test;
+} // namespace testing
+
+class ClawCappedStateFeedbackLoop : public StateFeedbackLoop<2, 1, 1> {
+ public:
+ ClawCappedStateFeedbackLoop(StateFeedbackLoop<2, 1, 1> &&loop)
+ : StateFeedbackLoop<2, 1, 1>(::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 goal in order to no longer
+ // saturate the controller.
+ double UnsaturateOutputGoalChange();
+
+ private:
+ double max_voltage_;
+};
class Claw
: public aos::controls::ControlLoop<control_loops::ClawQueue> {
public:
+ enum State {
+ // Waiting to receive data before doing anything.
+ UNINITIALIZED = 0,
+ // Estimating the starting location.
+ INITIALIZING = 1,
+ // Moving to find an index pulse.
+ ZEROING = 2,
+ // Normal operation.
+ RUNNING = 3,
+ // Internal error caused the claw to abort.
+ ESTOP = 4,
+ };
+
+ int state() { return state_; }
+
explicit Claw(
control_loops::ClawQueue *claw_queue = &control_loops::claw_queue);
- // Control loop time step.
- // Please figure out how to set dt from a common location
- // Please decide the correct value
- // Please use dt in your implementation so we can change looptimnig
- // and be consistent with legacy
- // And Brian please approve my code review as people are wait on
- // these files to exist and they will be rewritten anyway
- //static constexpr double dt;
-
protected:
virtual void RunIteration(
const control_loops::ClawQueue::Goal *goal,
@@ -34,8 +66,47 @@
control_loops::ClawQueue::Status *status);
private:
+ friend class testing::ClawTest_DisabledGoal_Test;
+ friend class testing::ClawTest_GoalPositiveWindup_Test;
+ friend class testing::ClawTest_GoalNegativeWindup_Test;
+
+ // Sets state_ to the correct state given the current state of the zeroing
+ // estimator.
+ void UpdateZeroingState();
+ void SetClawOffset(double offset);
+ // Corrects the Observer with the current position.
+ void Correct();
+
+ // Getter for the current claw position.
+ double claw_position() const;
+ // Our best guess at the current position of the claw.
+ double estimated_claw_position() const;
+ // Returns the current zeroing velocity for the claw. If the subsystem is too
+ // far away from the center, it will switch directions.
+ double claw_zeroing_velocity();
+
+ State state_ = UNINITIALIZED;
+
+ // The time when we last changed the claw piston state.
+ ::aos::time::Time last_piston_edge_;
+
// The state feedback control loop to talk to.
- ::std::unique_ptr<StateFeedbackLoop<2, 1, 1>> claw_loop_;
+ ::std::unique_ptr<ClawCappedStateFeedbackLoop> claw_loop_;
+
+ // Latest position from queue.
+ control_loops::ClawQueue::Position current_position_;
+ // Zeroing estimator for claw.
+ zeroing::ZeroingEstimator claw_estimator_;
+
+ // The goal for the claw.
+ double claw_goal_ = 0.0;
+ // Current velocity to move at while zeroing.
+ double claw_zeroing_velocity_ = 0.0;
+ // Offsets from the encoder position to the absolute position.
+ double claw_offset_ = 0.0;
+
+ // Whether claw was closed last cycle.
+ bool last_rollers_closed_ = false;
};
} // namespace control_loops