Added another pole to the flywheel
Scott, Adam and Campbel did a bunch of testing and concluded
that there is a second pole in the flywheel response. This
commit adds that pole to our system. We need to tune it,
but we need a robot first.
Change-Id: I5ba67db077bb709460c70f6df0164b8c3c1f700d
diff --git a/y2017/control_loops/superstructure/shooter/shooter.cc b/y2017/control_loops/superstructure/shooter/shooter.cc
index 18fc795..c0b9d6e 100644
--- a/y2017/control_loops/superstructure/shooter/shooter.cc
+++ b/y2017/control_loops/superstructure/shooter/shooter.cc
@@ -23,15 +23,16 @@
// TODO(austin): Pseudo current limit?
ShooterController::ShooterController()
- : loop_(new StateFeedbackLoop<3, 1, 1, StateFeedbackHybridPlant<3, 1, 1>,
- HybridKalman<3, 1, 1>>(
+ : loop_(new StateFeedbackLoop<4, 1, 1, StateFeedbackHybridPlant<4, 1, 1>,
+ HybridKalman<4, 1, 1>>(
superstructure::shooter::MakeIntegralShooterLoop())) {
history_.fill(0);
Y_.setZero();
}
void ShooterController::set_goal(double angular_velocity_goal) {
- loop_->mutable_next_R() << 0.0, angular_velocity_goal, 0.0;
+ loop_->mutable_next_R() << 0.0, angular_velocity_goal, angular_velocity_goal,
+ 0.0;
}
void ShooterController::set_position(double current_position) {
@@ -52,7 +53,7 @@
void ShooterController::Update(bool disabled, chrono::nanoseconds dt) {
loop_->mutable_R() = loop_->next_R();
- if (::std::abs(loop_->R(1, 0)) < 1.0) {
+ if (::std::abs(loop_->R(2, 0)) < 1.0) {
// Kill power at low angular velocities.
disabled = true;
}
@@ -72,17 +73,17 @@
static_cast<double>(kHistoryLength - 1));
// Ready if average angular velocity is close to the goal.
- error_ = average_angular_velocity_ - loop_->next_R(1, 0);
+ error_ = average_angular_velocity_ - loop_->next_R(2, 0);
- ready_ = std::abs(error_) < kTolerance && loop_->next_R(1, 0) > 1.0;
+ ready_ = std::abs(error_) < kTolerance && loop_->next_R(2, 0) > 1.0;
- if (last_ready_ && !ready_ && loop_->next_R(1, 0) > 1.0 && error_ < 0.0) {
+ if (last_ready_ && !ready_ && loop_->next_R(2, 0) > 1.0 && error_ < 0.0) {
needs_reset_ = true;
- min_velocity_ = loop_->X_hat(1, 0);
+ min_velocity_ = velocity();
}
if (needs_reset_) {
- min_velocity_ = ::std::min(min_velocity_, loop_->X_hat(1, 0));
- if (loop_->X_hat(1, 0) > min_velocity_ + 5.0) {
+ min_velocity_ = ::std::min(min_velocity_, velocity());
+ if (velocity() > min_velocity_ + 5.0) {
reset_ = true;
needs_reset_ = false;
}
@@ -109,10 +110,10 @@
void ShooterController::SetStatus(ShooterStatus *status) {
status->avg_angular_velocity = average_angular_velocity_;
- status->angular_velocity = X_hat_current_(1, 0);
+ status->angular_velocity = X_hat_current_(2, 0);
status->ready = ready_;
- status->voltage_error = X_hat_current_(2, 0);
+ status->voltage_error = X_hat_current_(3, 0);
status->position_error = position_error_;
status->instantaneous_velocity = dt_velocity_;
status->fixed_instantaneous_velocity = fixed_dt_velocity_;
diff --git a/y2017/control_loops/superstructure/shooter/shooter.h b/y2017/control_loops/superstructure/shooter/shooter.h
index d9b6f45..d13d94c 100644
--- a/y2017/control_loops/superstructure/shooter/shooter.h
+++ b/y2017/control_loops/superstructure/shooter/shooter.h
@@ -33,7 +33,8 @@
double voltage() const;
// Returns the instantaneous velocity.
- double velocity() const { return loop_->X_hat(1, 0); }
+ double velocity() const { return loop_->X_hat(2, 0); }
+ double voltage_error() const { return loop_->X_hat(3, 0); }
double dt_velocity() const { return dt_velocity_; }
@@ -50,7 +51,7 @@
Eigen::Matrix<double, 1, 1> Y_;
// The control loop.
::std::unique_ptr<StateFeedbackLoop<
- 3, 1, 1, StateFeedbackHybridPlant<3, 1, 1>, HybridKalman<3, 1, 1>>>
+ 4, 1, 1, StateFeedbackHybridPlant<4, 1, 1>, HybridKalman<4, 1, 1>>>
loop_;
// History array for calculating a filtered angular velocity.
@@ -67,7 +68,7 @@
double min_velocity_ = 0.0;
double position_error_ = 0.0;
- Eigen::Matrix<double, 3, 1> X_hat_current_;
+ Eigen::Matrix<double, 4, 1> X_hat_current_;
bool ready_ = false;
bool needs_reset_ = false;
diff --git a/y2017/control_loops/superstructure/superstructure_lib_test.cc b/y2017/control_loops/superstructure/superstructure_lib_test.cc
index 114b449..7906af4 100644
--- a/y2017/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2017/control_loops/superstructure/superstructure_lib_test.cc
@@ -29,10 +29,10 @@
namespace chrono = ::std::chrono;
using ::aos::monotonic_clock;
-class ShooterPlant : public StateFeedbackPlant<2, 1, 1> {
+class ShooterPlant : public StateFeedbackPlant<3, 1, 1> {
public:
- explicit ShooterPlant(StateFeedbackPlant<2, 1, 1> &&other)
- : StateFeedbackPlant<2, 1, 1>(::std::move(other)) {}
+ explicit ShooterPlant(StateFeedbackPlant<3, 1, 1> &&other)
+ : StateFeedbackPlant<3, 1, 1>(::std::move(other)) {}
void CheckU(const Eigen::Matrix<double, 1, 1> &U) override {
EXPECT_LE(U(0, 0), U_max(0, 0) + 0.00001 + voltage_offset_);
@@ -224,7 +224,7 @@
double intake_position() const { return intake_plant_->X(0, 0); }
double intake_velocity() const { return intake_plant_->X(1, 0); }
- double shooter_velocity() const { return shooter_plant_->X(1, 0); }
+ double shooter_velocity() const { return shooter_plant_->X(2, 0); }
double indexer_velocity() const { return column_plant_->X(1, 0); }