Designed a shooter loop using voltage error estimation.
This gets rid of all the corner cases with the moving position style
loop from before, and doesn't have overshoot issues like standard
integral loops have.
Change-Id: I4e4eb1767038563cf851040ce8218e73ca60904a
diff --git a/y2016/control_loops/shooter/shooter.h b/y2016/control_loops/shooter/shooter.h
index 6a3fa4d..0354bb5 100644
--- a/y2016/control_loops/shooter/shooter.h
+++ b/y2016/control_loops/shooter/shooter.h
@@ -6,61 +6,59 @@
#include "aos/common/controls/control_loop.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "y2016/control_loops/shooter/shooter_plant.h"
+#include "y2016/control_loops/shooter/shooter_integral_plant.h"
#include "y2016/control_loops/shooter/shooter.q.h"
namespace y2016 {
namespace control_loops {
+namespace shooter {
namespace {
-// TODO(constants): Update.
-const double kTolerance = 10.0;
-const double kMaxSpeed = 10000.0 * (2.0 * M_PI) / 60.0 * 15.0 / 34.0;
-const double kAngularVelocityWeightScalar = 0.35;
+constexpr double kTolerance = 10.0;
} // namespace
-struct ShooterStatus {
- double avg_angular_velocity;
- bool ready;
-};
-
class ShooterSide {
public:
ShooterSide();
- void SetGoal(double angular_velocity_goal);
- void EstimatePositionTimestep();
- void SetPosition(double current_position);
- const ShooterStatus GetStatus();
- double GetOutput();
- void UpdateLoop(bool output_is_null);
+ // 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(ShooterSideStatus *status);
+
+ // Returns the control loop calculated voltage.
+ double voltage() const;
+
+ // Executes the control loop for a cycle.
+ void Update(bool disabled);
private:
- ::std::unique_ptr<StateFeedbackLoop<2, 1, 1>> loop_;
-
- double current_position_ = 0.0;
- double position_goal_ = 0.0;
- double angular_velocity_goal_ = 0.0;
+ // 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 const int kHistoryLength = 5;
- double history_[kHistoryLength];
+ static constexpr int kHistoryLength = 10;
+ ::std::array<double, kHistoryLength> history_;
ptrdiff_t history_position_;
DISALLOW_COPY_AND_ASSIGN(ShooterSide);
};
-class Shooter
- : public ::aos::controls::ControlLoop<control_loops::ShooterQueue> {
+class Shooter : public ::aos::controls::ControlLoop<ShooterQueue> {
public:
- explicit Shooter(control_loops::ShooterQueue *shooter_queue =
- &control_loops::shooter_queue);
+ explicit Shooter(
+ ShooterQueue *shooter_queue = &control_loops::shooter::shooter_queue);
protected:
- void RunIteration(const control_loops::ShooterQueue::Goal *goal,
- const control_loops::ShooterQueue::Position *position,
- control_loops::ShooterQueue::Output *output,
- control_loops::ShooterQueue::Status *status) override;
+ void RunIteration(const ShooterQueue::Goal *goal,
+ const ShooterQueue::Position *position,
+ ShooterQueue::Output *output,
+ ShooterQueue::Status *status) override;
private:
ShooterSide left_, right_;
@@ -68,6 +66,7 @@
DISALLOW_COPY_AND_ASSIGN(Shooter);
};
+} // namespace shooter
} // namespace control_loops
} // namespace y2016